package dispatcher

import (
	"container/heap"
	"context"
	"encoding/json"
	"sort"
	"strconv"
	"sync"
	"time"

	"dev.ixmxm.top/rms/config"
	"dev.ixmxm.top/rms/managers/chargemanager"
	"dev.ixmxm.top/rms/managers/robotmanager"

	"dev.ixmxm.top/rms/boot"
	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/model"
	"dev.ixmxm.top/rms/proto"
	"dev.ixmxm.top/rms/utils/log"
	"dev.ixmxm.top/rms/utils/tools"
	"github.com/pkg/errors"
	"go.mongodb.org/mongo-driver/bson"
)

type GetMapSubtasks func(ctx context.Context, robot *model.Robot, task *model.Task) ([]model.Subtask, float64, codes.Code)

//检查已登记的任务进行处理（只解析无主任务或者有主任务号下的第一条任务）（已登记-->已解析）
func (m *DispatchManager) CheckRegisterTask() {
	for {
		func() {
			ctx := context.Background()
			// t1 := time.Now()
			// log.Infof("机器人分配任务开始")
			canUseRobots := m.getCanUseRobots(ctx)
			if canUseRobots == nil || len(canUseRobots) < 1 {
				// log.Infof("没有可用机器人")
				return
			}
			robotsMap, err := m.getRobotsMapsMap(ctx, canUseRobots)
			if err != nil {
				log.Errorf("获取机器人和地图的map错误error%s", err.Error())
			}
			if robotsMap == nil || len(robotsMap) < 1 {
				return
			}
			// log.Warnf("当前地图和车匹配关系:%+v", robotsMap)

			var wg sync.WaitGroup
			for mapName, r := range robotsMap {
				robots := r
				if robots == nil || len(robots) < 1 {
					continue
				}
				wg.Add(1)
				go m.RobotMatches(ctx, mapName, robots, &wg)
			}
			wg.Wait()
			// log.Infof("机器人分配任务结束,本轮分配耗时:%dms", time.Now().UnixMilli()-t1.UnixMilli())
		}()
		tools.Sleep(0.2)
	}
}

func (m *DispatchManager) processTask(ctx context.Context, robot *model.Robot, task *model.Task, subtasks []model.Subtask) codes.Code {
	// canReach := m.CheckDeadWayCanReach(task, robot)
	// if !canReach {
	// 	return codes.ErrGoToDeadWay
	// }
	ctxTx := boot.Begin(ctx)
	var err codes.Code
	//添加任务绑定
	err = m.TM().AddMainTaskToDB(ctxTx, task.MainTaskCode, task.Id, robot.Account.AgvId)
	if err != nil {
		log.Errorf("任务分车，数据库添加任务绑定失败  taskid:%d", task.Id)
		ctxTx.Rollback()
		return err
	}
	//保存子任务
	for i, subtsk := range subtasks {
		subtsk.Index = i
		subtsk.MainTaskTargetSiteID = task.TargetSiteID
		subtsk.MainTaskCode = task.MainTaskCode
		_, err := m.TM().AddSubtaskToDB(ctxTx, &subtsk)
		if err != nil {
			log.Errorf("任务分车，数据库添加子任务失败  task:%d", task.Id)
			ctxTx.Rollback()
			return err
		}
	}

	//更改任务状态为已解析
	err = m.TM().UpdateStatusAgvIdToDB(ctxTx, task.Id, model.TaskStasusResolved, robot.Account.AgvId)
	if err != nil {
		log.Errorf("任务分车，数据库更改任务状态为已解析失败  taskid:%d", task.Id)
		ctxTx.Rollback()
		return err
	}

	ctxTx.Commit()
	log.Infof("task %d 分配车 %s 成功", task.Id, robot.Account.AgvId)
	return nil
}
func getGroudLocationIndex(targetId string, depthPointId []string) int {
	for i, p := range depthPointId {
		if p == targetId {
			return i
		}
	}
	return 999
}
func (m *DispatchManager) exchangeTask(ctx context.Context, subtask *model.Subtask) codes.Code {
	processTask := func(ctx context.Context, robot *model.Robot, task *model.Task, checkflg int) codes.Code {
		//清除全局路径(只清除路径，不清除锁)
		if errDeletePath := m.TRM().DeleteRobotGlobalPaths(ctx, task.AgvId, task.MapName); errDeletePath != nil {
			log.Errorf("数据库删除全局路径失败  taskid:%d robot:%s", task.Id, task.AgvId)
			return errDeletePath
		}
		//删除对应子任务
		err0 := m.TM().DeleteSubtasksToDB(ctx, task.Id)
		if err0 != nil {
			log.Errorf("数据库删除子任务失败  taskid:%d", task.Id)
			return err0
		}

		subtasks, err, _ := m.GetSubTasks(ctx, task, robot)
		if err != nil {
			return err
		}
		if subtasks == nil || len(subtasks) < 1 {
			log.Errorf("模板生成子任务错误")
			return codes.ErrSubtasksIsNull
		}
		//添加任务绑定
		err = m.TM().AddMainTaskToDB(ctx, task.MainTaskCode, task.Id, robot.Account.AgvId)
		if err != nil {
			log.Errorf("数据库添加任务绑定失败  taskid:%d", task.Id)
			return err
		}
		//保存子任务
		for i, subtsk := range subtasks {
			subtsk.Index = i
			subtsk.MainTaskTargetSiteID = task.TargetSiteID
			subtsk.MainTaskCode = task.MainTaskCode
			_, err := m.TM().AddSubtaskToDB(ctx, &subtsk)
			if err != nil {
				log.Errorf("数据库添加子任务失败  task:%d", task.Id)
				return err
			}
		}
		//更改任务状态为已解析
		err = m.TM().UpdateStatusAgvIdToDB(ctx, task.Id, model.TaskStasusResolved, robot.Account.AgvId)
		if err != nil {
			log.Errorf("数据库更改任务状态为已解析失败  taskid:%d", task.Id)
			return err
		}
		//如果是取货任务发生置换，对应的放货任务需要修改分配的AGV
		if checkflg == 1 && task.MainTaskCode != "" {
			err = m.TM().UpdateTaskAgvIdToDB(ctx, task.MainTaskCode, robot.Account.AgvId)
			if err != nil {
				log.Errorf("数据库更改放货任务分配AGV失败  MainTaskCode:%s", task.MainTaskCode)
				return err
			}
		}
		return nil
	}

	if subtask.CheckFlg == 1 || subtask.CheckFlg == 2 {
		//根据子任务查询获取主任务
		task, err := m.TM().GetTaskFromDB(ctx, subtask.TaskId)
		if err != nil {
			return err
		}
		if subtask.CheckFlg == 2 {
			if gLocation := m.WMM().FindGroundLocation(task.MapName, task.TargetSiteID); gLocation != nil {
				tasks, _ := m.TM().GetAllSameGroundLocationTaskFromDB(ctx, task.MapName, task.Id, gLocation.DepthPointId)
				if len(tasks) > 0 {
					for _, t := range tasks {
						if t.Status != model.TaskStatusRegister && t.Status != model.TaskStatusAssign {
							continue
						}
						if t.CreatedTime.Sub(task.CreatedTime) > 0 {
							continue
						}
						log.Errorf("robot:%s 当前任务task%d 查到替换任务 robot:%s 放货任务%d还未开始，检查主任务%s对应的取货任务是否执行异常", task.AgvId, task.Id, t.AgvId, t.Id, t.MainTaskCode)
						return codes.ErrTaskStatusNotMatch
					}
				}
			}
		}
		//查找是否存在置换任务
		otherSubtasks, err1 := m.TM().GetAllSamePointSubtaskFromDB(ctx, subtask.MapName, subtask.Id, subtask.TargetSiteID)
		if err1 != nil {
			return err1
		}
		if otherSubtasks == nil || len(otherSubtasks) == 0 {
			return nil
		}
		firstTask := task
		findExchangeTask := false
		var clearSubtaskId uint64
		for _, os := range otherSubtasks {
			tasktmp, _ := m.TM().GetTaskFromDB(ctx, os.TaskId)
			if tasktmp.CreatedTime.Sub(firstTask.CreatedTime) > 0 || tasktmp.Type != firstTask.Type {
				continue
			} else if subtask.CheckFlg == os.CheckFlg {
				if subtask.CheckFlg == 2 {
					//判断是否是同一列
					node1 := m.TRM().GetNode(firstTask.MapName, firstTask.TargetSiteID)
					node2 := m.TRM().GetNode(tasktmp.MapName, tasktmp.TargetSiteID)
					if m.WMM().CheckSameGroundLocation(firstTask.MapName, node1, node2) {
						if tasktmp.Status != model.TaskStatusRunning {
							log.Errorf("robot:%s 放货任务%d还未开始，检查主任务%s对应的取货任务是否执行异常", tasktmp.AgvId, tasktmp.Id, tasktmp.MainTaskCode)
							return codes.ErrTaskStatusNotMatch
						}
						firstTask = tasktmp
						findExchangeTask = true
						clearSubtaskId = os.Id
					}
				} else {
					firstTask = tasktmp
					findExchangeTask = true
					clearSubtaskId = os.Id
				}
			}
		}
		if findExchangeTask {
			log.Infof("查到替换任务 robot:%s 当前任务task%d 替换 robot:%s执行的任务task%d", task.AgvId, task.Id, firstTask.AgvId, firstTask.Id)

			//把当前机器人的任务切换成最早的任务
			robot := robotmanager.GetFromCache(task.AgvId)
			errCode := processTask(ctx, robot, firstTask, subtask.CheckFlg)
			if errCode != nil {
				return errCode
			}
			//把当前任务分配机器人切换成最早的任务分配的机器人
			robot1 := robotmanager.GetFromCache(firstTask.AgvId)
			errCode = processTask(ctx, robot1, task, subtask.CheckFlg)
			if errCode != nil {
				return errCode
			}
			//机器人任务操作
			if caller, _ := m.GetCaller(firstTask.AgvId); caller != nil {
				errOperate := caller.OperateTask(ctx, clearSubtaskId, m.TM(), proto.TaskOperatePb_TaskClear)
				if errOperate != nil {
					log.ErrorfWithContext(ctx, "operate task error:%s", errOperate.UnWrapError())
					return errOperate
				}
			}
			log.Infof("robot:%s 当前任务task%d 替换 robot:%s执行的任务task%d  成功", task.AgvId, task.Id, firstTask.AgvId, firstTask.Id)
		}
	} else if subtask.CheckFlg == 3 {
		//根据子任务查询获取主任务
		task, err := m.TM().GetTaskFromDB(ctx, subtask.TaskId)
		if err != nil {
			return err
		}
		firstTask := task
		findEarlierTask := false
		if gLocation := m.WMM().FindGroundLocation(task.MapName, task.TargetSiteID); gLocation != nil {
			tasks, _ := m.TM().GetAllSameGroundLocationTaskFromDB(ctx, task.MapName, task.Id, gLocation.DepthPointId)
			if len(tasks) > 0 {
				log.Infof("gLocation.DepthPointId %v", gLocation.DepthPointId)
				for _, t := range tasks {
					id1 := getGroudLocationIndex(firstTask.TargetSiteID, gLocation.DepthPointId)
					id2 := getGroudLocationIndex(t.TargetSiteID, gLocation.DepthPointId)
					if id2 < id1 {
						log.Infof("firstTask(点位%s id1 %d) t(点位%s id2 %d)", firstTask.TargetSiteID, id1, t.TargetSiteID, id2)
						firstTask = t
						findEarlierTask = true
					}
				}
			}
		}
		if findEarlierTask {
			log.Infof("robot:%s task%d(点位%s 最初%s) 查到最早的同列放货任务%d(点位%s 最初%s)", task.AgvId, task.Id, task.TargetSiteID, task.TargetID, firstTask.Id, firstTask.TargetSiteID, firstTask.TargetID)
			err = m.TM().UpdateTaskTargetToDB(ctx, task.Id, firstTask.TargetSiteID)
			if err != nil {
				log.Errorf("数据库更改任务目标点失败  taskid:%d ", task.Id)
				return err
			}
			err = m.TM().UpdateSubtaskTargetToDB(ctx, subtask.TaskId, task.TargetSiteID, firstTask.TargetSiteID)
			if err != nil {
				log.Errorf("数据库更改子任务目标点失败  taskid:%d ", task.Id)
				return err
			}
			err = m.TM().UpdateTaskTargetToDB(ctx, firstTask.Id, task.TargetSiteID)
			if err != nil {
				log.Errorf("数据库更改任务目标点失败  firstTaskid:%d ", firstTask.Id)
				return err
			}
			if firstTask.Status != model.TaskStatusRegister && firstTask.Status != model.TaskStatusAssign {
				err = m.TM().UpdateSubtaskTargetToDB(ctx, firstTask.Id, firstTask.TargetSiteID, task.TargetSiteID)
				if err != nil {
					log.Errorf("数据库更改子任务目标点失败  firstTaskid:%d ", firstTask.Id)
					return err
				}
			}
			log.Infof("互换点位成功!")
		}
	}
	return nil
}

// RobotMatches 在同一地图中做车和订单匹配
func (m *DispatchManager) RobotMatches(ctx context.Context, mapName string, robots []*model.Robot, wg *sync.WaitGroup) {
	haveEarlierTask := func(task *model.Task) bool {
		findEarlierTask := false
		node := m.TRM().GetNode(task.MapName, task.TargetSiteID)
		if node.Type == model.NodeStation && node.LocationType == model.LocationGround {
			if gLocation := m.WMM().FindGroundLocation(task.MapName, task.TargetSiteID); gLocation != nil {
				tasks, _ := m.TM().GetAllSameGroundLocationPickTaskFromDB(ctx, task.MapName, task.Id, gLocation.DepthPointId)
				if len(tasks) > 0 {
					log.Infof("当前任务id:%d 任务目标点 %s gLocation.DepthPointId %v", task.Id, task.TargetSiteID, gLocation.DepthPointId)
					for _, t := range tasks {
						id1 := getGroudLocationIndex(task.TargetSiteID, gLocation.DepthPointId)
						id2 := getGroudLocationIndex(t.TargetSiteID, gLocation.DepthPointId)
						if id2 > id1 {
							log.Infof("更早任务id:%d 任务目标点 %s", t.Id, t.TargetSiteID)
							findEarlierTask = true
							break
						}
					}
				}
			}
		}
		return findEarlierTask
	}
	defer wg.Done()
	robotCount := int64(len(robots))
	var robotIds []string
	for _, r := range robots {
		robotIds = append(robotIds, r.Account.AgvId)
	}
	if robotCount < 1 {
		return
	}
	if len(robotIds) > 0 {
		// log.Infof("地图:%s开始匹配订单和机器人,可用机器人%+v 总计：%d", mapName, robotIds, len(robotIds))
	}
	staticTask, err := m.mc.TM.GetTasksByMapAndAgvIDStatus(ctx, mapName, []model.TaskStatus{model.TaskStatusRegister}, robotIds, nil)
	if err != nil {
		log.Errorf("获取地图: %s 下指定车的任务出错错误error%s", mapName, err.Error())
		return
	}

	getCount := config.Conf.TaskCfg.MatchMultiple * robotCount
	tasks, err := m.mc.TM.GetTasksByMapAndStatus(ctx, mapName, []model.TaskStatus{model.TaskStatusRegister}, &getCount)
	if err != nil {
		log.Errorf("获取地图: %s 下的任务出错错误error%s", mapName, err.Error())
		return
	}

	newTasks := staticTask
	if staticTask != nil {
		newTasks = append(staticTask, tasks...)
	}

	if newTasks == nil || len(newTasks) < 1 {
		// log.Warnf("地图:%s下没有task需要处理", mapName)
		return
	}
	//--------------正常任务处理--------------------
	dealNomalTask := func(task *model.Task) bool {
		robot, subtasks := m.findBestRobot(ctx, task, robots, false)
		if robot == nil || subtasks == nil {
			if m.TRM().CrossMapCanReach(mapName, task.MapName) {
				robot, subtasks = m.findBestRobot(ctx, task, robots, true)
				if robot == nil || subtasks == nil {
					return false
				}
			} else {
				return false
			}
		}
		// log.Warnf("task:%d找到可用的机器人:%s, subtask:%+v", task.Id, robot.Account.AgvId, subtasks)
		//任务处理
		mainTask, _ := m.mc.TM.GetMainTaskByAgvFromDB(ctx, robot.Account.AgvId)
		if mainTask != nil && task.Type != model.TaskTypeAvoid {
			return false
		}

		errCode := m.processTask(ctx, robot, task, subtasks)
		if errCode != nil {
			return false
		}
		return true
	}
	//记录相同目标点
	for _, task := range newTasks {
		log.Infof("地图:%s 开始给task%d匹配机器人", mapName, task.Id)
		//严格按照顺序执行
		if task.MainTaskCode != "" {
			firstTask, err := m.mc.TM.GetFirstTaskByMainTaskCodeFromDB(ctx, task.MainTaskCode)
			if err != nil {

				log.Errorf("获取主任务号%s下的第一条任务失败，error:%s", task.MainTaskCode, err.Error())
				continue
			}
			if firstTask == nil {
				log.Errorf("获取主任务号%s下的第一条任务失败", task.MainTaskCode)
				continue
			}
			if firstTask.Id != task.Id {
				// log.Warnf("mainTask:%s 当前task为%d非%d", task.MainTaskCode, firstTask.Id, task.Id)
				continue
			}
			if haveEarlierTask(task) {
				log.Warnf("mainTask:%s 当前task%d有更早的地堆库任务", task.MainTaskCode, task.Id)
				continue
			}
		}
		if task.Type == model.TaskTypeAvoid {
			if task.RetryCnt > 3 {
				ctxTx := boot.Begin(ctx)
				err := m.mc.TM.UpdateTaskStatusToDB(ctxTx, task.Id, model.TaskStatusCanceled)
				if err != nil {
					ctxTx.Rollback()
				} else {
					log.Infof("robot:%s 空闲避让任务:%d  重归划次数过多，直接取消!!!", task.AgvId, task.Id)
					ctxTx.Commit()
				}
				continue
			} else {
				data := bson.M{"retry_cnt": task.RetryCnt + 1}
				m.db.Collection(ctx, model.Task{}.TableName()).FindOneAndUpdate(ctx, bson.M{"id": task.Id}, bson.M{"$set": data})
			}

		}

		log.Infof("地图%s开始给task%d匹配机器人%v", mapName, task.Id, robotIds)
		//判断当前任务是否存在依赖，依赖任务是否已经结束
		if task.DependExternalTaskCode != "" {
			//判断依赖任务是否已经结束
			dependTask, errDependTask := m.mc.TM.GetDependExternalUnfinishTask(ctx, task.DependExternalTaskCode)
			if errDependTask != nil {
				log.Errorf("查询依赖任务错误:%s,！！！", errDependTask.Error())
				continue
			}
			if dependTask != nil {
				log.Errorf("RMS任务：%d,存在依赖任务:%d还未完成！！！", task.Id, dependTask.Id)
				continue
			}
		}
		dealNomalTask(task)
	}

}

//检查已分配的主任务下任务分车（已登记-->已分配）
func (m *DispatchManager) CheckAssignMainTask() {
	processTask := func(ctx context.Context, mainTask *model.MainTask, tasks []*model.Task) codes.Code {
		for _, task := range tasks {
			//更改任务状态为已分配
			err := m.TM().UpdateStatusAgvIdToDB(ctx, task.Id, model.TaskStatusAssign, mainTask.AgvId)
			if err != nil {
				log.Errorf("已分配的任务进行处理，数据库更改任务状态为已分配失败  taskid:%d", task.Id)
				return err
			}
		}
		return nil
	}
	processTask2 := func(ctx context.Context, tasks []*model.Task) codes.Code {
		for _, task := range tasks {
			//更改任务状态为已分配
			err := m.TM().UpdateStatusAgvIdToDB(ctx, task.Id, model.TaskStatusAssign, task.AgvId)
			if err != nil {
				log.Errorf("已分配的任务进行处理，数据库更改任务状态为已分配失败  taskid:%d", task.Id)
				return err
			}
		}
		return nil
	}

	ctx := context.Background()
	for {
		tools.Sleep(3)
		//获取获取已分配的主任务
		mainTasks, err := m.mc.TM.GetAllMainTasksFromDB(ctx)
		if err != nil {
			log.Errorf("获取已分配的主任务失败，error:%s", err.Error())
			continue
		}

		for _, mainTask := range mainTasks {
			if mainTask.MainTaskCode == "" {
				continue
			}
			//下发未分车的任务
			registerTasks := make([]*model.Task, 0)
			//下发已分车的任务
			registerTasks2 := make([]*model.Task, 0)
			//获取主任务下的所有任务
			tasks, err := m.TM().GetAllTasksByMainTaskCodeFromDB(ctx, mainTask.MainTaskCode)
			if err != nil {
				log.Errorf("获取主任务下的所有任务失败，error:%s", err.Error())
				continue
			}
			for _, task := range tasks {
				//下发未分车的任务处理
				if task.AgvId == "" && task.Status == model.TaskStatusRegister {
					registerTasks = append(registerTasks, task)
				}
				//下发已分车的任务处理
				if task.AgvId != "" && task.Status == model.TaskStatusRegister {
					registerTasks2 = append(registerTasks2, task)
				}
			}

			if len(registerTasks) > 0 {
				//未分车的任务处理
				err = processTask(ctx, mainTask, registerTasks)
				if err != nil {
					continue
				}
			}

			if len(registerTasks2) > 0 {
				//已分车的任务处理
				err = processTask2(ctx, registerTasks2)
				if err != nil {
					continue
				}
			}
		}
	}
}

//检查已分配的任务进行处理（已分配-->已解析）
func (m *DispatchManager) CheckAssignTask() {
	//拆分任务
	processTask := func(ctx context.Context, robot *model.Robot, task *model.Task) codes.Code {
		// canReach := m.CheckDeadWayCanReach(task, robot)
		// if !canReach {
		// 	return codes.ErrGoToDeadWay
		// }
		log.Warnf("处理已经分配的任务,rms_task_id:%d，robot:%s", task.Id, robot.Account.AgvId)
		startTime := time.Now()
		defer func() {
			log.Warnf("taskId:%s handle findBestRobot time %dms", task.Id, time.Now().UnixMilli()-startTime.UnixMilli())
		}()
		if task.MainTaskCode != "" {
			taskNew, err := m.mc.TM.GetRobotIdleAvoidTaskFromDB(ctx, robot.Account.AgvId)
			if taskNew == nil || taskNew.MainTaskCode == "" {
				taskNew, err = m.mc.TM.GetFirstWaitingTaskByMainTaskCodeFromDB(ctx, task.MainTaskCode)
				if err != nil {
					log.Errorf("检查已分配的任务进行处理，获取主任务下第一条未完成的任务失败 err:%s", err.Error())
					return err
				}
			}
			if taskNew.Type != model.TaskTypeAvoid && taskNew.Id != task.Id {
				return codes.ErrTaskNotExists
			}
			if !robot.StatusIsExists() {
				return codes.ErrDeviceStatusNotExists
			}
			subtasks := make([]model.Subtask, 0)
			if robot.Status.MapName != task.MapName {
				elevators := m.DM().GetAllElevatorFromCache()
				allRobots := robotmanager.GetAllRobotsFromCache()
				goalList, _, err := m.TRM().GetCrossPath(ctx, robot, task.MapName, task.TargetSiteID, elevators, allRobots)
				if err != nil {
					log.Errorf("get cross path error %s", err)
					return err
				}
				//模板生成子任务
				subtasks, err = m.GetCrossMapSubTasks(ctx, goalList, task, robot)
				if err != nil {
					log.Errorf("get cross map sub task error:%s", err.Error())
					return err
				}
			} else {
				//获取子任务
				subtasks, err, _ = m.GetSubTasks(ctx, task, robot)
				if err != nil {
					log.Errorf("检查已分配的任务进行处理，获取子任务失败 err:%s", err.Error())
					return err
				}
			}
			if len(subtasks) < 1 {
				log.Errorf("检查已分配的任务:%d,进行处理，获取子任务失败 nil", task.Id)
				return codes.ErrSubtaskError
			}
			if _, success := m.SubtaskAvailableRobotCheck(ctx, robot, &subtasks[0]); !success {
				log.Warnf("robot:%s ,已经分配的任务:%d ,路径规划失败!", robot.Account.AgvId, &subtasks[0].TaskId)
				return codes.ErrPathPlanError
			} else if taskNew.Type == model.TaskTypeAvoid && taskNew.Id != task.Id {
				runningSubtask, _ := m.mc.TM.GetRunningSubtaskFromDB(ctx, robot.Account.AgvId)
				if runningSubtask != nil && runningSubtask.TaskId == taskNew.Id {
					if robot.StatusIsExists() && (m.RobotGpathsLess(ctx, robot.Account.AgvId) || robot.Status.SiteIdS == runningSubtask.TargetSiteID) {
						log.Infof("给小车(%s)下发避让任务(%d)剩余路径短，不进行中断", robot.Account.AgvId, runningSubtask.TaskId)
						return codes.ErrPathPlanError
					}
					log.Infof("任务:%d,尝试中断避让任务,机器人:%s状态:%d", runningSubtask.Id, robot.Account.AgvId, robot.Status.Status)
					if errBreak := m.breakTask(runningSubtask, nil); errBreak != nil {
						log.Errorf("break robot:%s task:%d error:%s", robot.Account.AgvId, runningSubtask.Id, errBreak.UnWrapError())
					}
				}
				return codes.ErrPathPlanError
			}

			ctxTx := boot.Begin(ctx)
			//保存子任务
			for i, subtsk := range subtasks {
				subtsk.Index = i
				subtsk.MainTaskTargetSiteID = task.TargetSiteID
				subtsk.MainTaskCode = task.MainTaskCode
				_, err := m.TM().AddSubtaskToDB(ctxTx, &subtsk)
				if err != nil {
					log.Errorf("已分配的任务进行处理，数据库添加子任务失败  task:%d", task.Id)
					ctxTx.Rollback()
					return err
				}
			}
			//添加任务绑定
			err = m.TM().AddMainTaskToDB(ctxTx, task.MainTaskCode, task.Id, robot.Account.AgvId)
			if err != nil {
				log.Errorf("已分配的任务进行处理，数据库添加任务绑定失败  taskid:%d", task.Id)
				ctxTx.Rollback()
				return err
			}
			//更改任务状态为已解析
			err = m.TM().UpdateStatusAgvIdToDB(ctxTx, task.Id, model.TaskStasusResolved, robot.Account.AgvId)
			if err != nil {
				log.Errorf("已分配的任务进行处理，数据库更改任务状态为已解析失败  taskid:%d", task.Id)
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}

		return nil
	}
	cancelTask := func(ctx context.Context, assignTask *model.Task) {
		ctxTx := boot.Begin(ctx)
		err := m.mc.TM.UpdateTaskStatusToDB(ctxTx, assignTask.Id, model.TaskStatusCanceled)
		if err != nil {
			ctxTx.Rollback()
		} else {
			log.Infof("robot:%s 空闲避让任务:%d  重归划次数过多，直接取消!!!", assignTask.AgvId, assignTask.Id)
			ctxTx.Commit()
		}
	}
	ctx := context.Background()
	for {
		tools.Sleep(1)
		//获取获取已分配的任务
		assignTasks, err := m.mc.TM.GetAllStatusTasksFromDB(ctx, []model.TaskStatus{model.TaskStatusAssign})
		if err != nil {
			log.Errorf("获取已分配的任务失败，error:%s", err.Error())
			continue
		}

		for _, assignTask := range assignTasks {
			robot := robotmanager.GetFromCache(assignTask.AgvId)
			if !robot.StatusIsExists() || !robot.Status.TaskOnline {
				continue
			}
			//判断有没有任务绑定
			mainTask, err := m.mc.TM.GetMainTaskByAgvFromDB(ctx, assignTask.AgvId)
			if err != nil {
				log.Errorf("获取任务(%d)绑定失败，error:%s", assignTask.Id, err.Error())
				continue
			}
			if mainTask == nil && assignTask.Type == model.TaskTypeAvoid {
				ctxTx := boot.Begin(ctx)
				err := m.mc.TM.UpdateTaskStatusToDB(ctxTx, assignTask.Id, model.TaskStatusCanceled)
				if err != nil {
					ctxTx.Rollback()
				} else {
					log.Infof("robot:%s 空闲避让任务:%d  绑定的主任务不存在，直接取消!!!", assignTask.AgvId, assignTask.Id)
					ctxTx.Commit()
				}
			}
			avoidTask := false
			if mainTask != nil && mainTask.TaskId != 0 {
				if task, _ := m.mc.TM.GetTaskFromDB(ctx, mainTask.TaskId); task != nil && task.Type == model.TaskTypeAvoid {
					avoidTask = true
				}
			}
			if assignTask.Type == model.TaskTypeAvoid && mainTask != nil && mainTask.MainTaskCode != assignTask.MainTaskCode {
				cancelTask(ctx, assignTask)
				continue
			}
			//跳出：有任务绑定和主任务号相同和任务号不同
			if mainTask != nil && mainTask.MainTaskCode == assignTask.MainTaskCode && (mainTask.TaskId == 0 || avoidTask) {
				if assignTask.Type == model.TaskTypeAvoid {
					if assignTask.RetryCnt > 3 {
						cancelTask(ctx, assignTask)
						continue
					} else {
						data := bson.M{"retry_cnt": assignTask.RetryCnt + 1}
						m.db.Collection(ctx, model.Task{}.TableName()).FindOneAndUpdate(ctx, bson.M{"id": assignTask.Id}, bson.M{"$set": data})
					}
				}
				//任务处理
				log.Warnf("处理已经分配的任务:%d", assignTask.Id)
				err = processTask(ctx, robot, assignTask)
				if err != nil {
					log.Errorf("process task:%d err:%s", assignTask.Id, err)
					continue
				}

				// break
			}
		}
	}
}

type taskRecord struct {
	taskId    uint64
	startTime time.Time
}

func (m *DispatchManager) TargetHaveIdleRobot(t *model.Task) bool {
	agvs := robotmanager.GetAllTaskRobotsFromCache()
	targetNodes := make(map[string]string, 0)
	target_node := m.mc.TRM.GetNode(t.MapName, t.TargetSiteID)
	targetNodes[t.TargetSiteID] = t.TargetSiteID
	for _, ban := range target_node.BanNodes {
		if ban != "0" {
			targetNodes[ban] = ban
		}
	}
	for _, a := range agvs {
		if a.Account.AgvId == t.AgvId {
			continue
		}
		//不在同一地图
		if a.Account.MapName != t.MapName {
			continue
		}
		//检测小车存在未完成的子任务
		subtask, _ := m.mc.TM.GetWaitingSubtaskFromDB(context.Background(), a.Account.AgvId)
		if subtask != nil && subtask.RetryCount > 5 {
			continue
		}
		ids := robotmanager.GetLocalSitesFromDB(context.Background(), a.Account.AgvId)
		for _, n := range ids {
			if _, find := targetNodes[n]; find {
				return true
			}
		}
	}
	return false
}
func (m *DispatchManager) SubtargetHaveIdleRobot(t *model.Subtask, agvs []*model.Robot) bool {
	targetNodes := make(map[string]string, 0)
	target_node := m.mc.TRM.GetNode(t.MapName, t.TargetSiteID)
	targetNodes[t.TargetSiteID] = t.TargetSiteID
	for _, ban := range target_node.BanNodes {
		if ban != "0" {
			targetNodes[ban] = ban
		}
	}
	for _, a := range agvs {
		if a.Account.AgvId == t.AgvId {
			continue
		}
		//不在同一地图
		if a.Account.MapName != t.MapName {
			continue
		}
		//检测小车存在未完成的子任务
		subtask, _ := m.mc.TM.GetWaitingSubtaskFromDB(context.Background(), a.Account.AgvId)
		if subtask != nil && subtask.RetryCount > 5 {
			continue
		}
		ids := robotmanager.GetLocalSitesFromDB(context.Background(), a.Account.AgvId)
		for _, n := range ids {
			if _, find := targetNodes[n]; find {
				return true
			}
		}
	}
	return false
}

//检查已解析的子任务进行处理（已解析任务下发）
func (m *DispatchManager) CheckRunSubtask() {
	ctx := context.Background()
	// startWaitTime := make(map[string]taskRecord, 0)
	for {
		tools.Sleep(0.2)
		//获取所有可用AGV
		robots := robotmanager.GetAllTaskRobotsFromCache()
		if len(robots) <= 0 {
			continue
		}
		for _, robot := range robots {
			if !robot.StatusIsExists() || !robot.Status.TaskOnline || !robot.Status.Online {
				continue
			}

			//检测小车存在未完成的子任务不可下发子任务
			runTask, _ := m.mc.TM.GetRunningSubtaskFromDB(context.Background(), robot.Account.AgvId)
			if runTask != nil {
				continue
			}
			//子任务完成处理没有执行不可进行下一条子任务
			st, _ := m.TM().GetRobotFinishProcessSubtaskFromDB(ctx, robot.Account.AgvId)
			if st != nil {
				continue
			}

			//判断有没有任务绑定
			mainTask, err := m.mc.TM.GetMainTaskByAgvFromDB(ctx, robot.Account.AgvId)
			if err != nil {
				log.Errorf("获取任务绑定失败，robot:%s error:%s", robot.Account.AgvId, err.Error())
				continue
			}

			subtask, _ := m.mc.TM.GetWaitingSubtaskFromDB(ctx, robot.Account.AgvId)
			if subtask == nil {
				continue
			}

			//获取任务
			task, _ := m.mc.TM.GetTaskFromDB(ctx, subtask.TaskId)
			if task == nil || task.Status.IsPause() {
				continue
			}

			// if m.SubtargetHaveIdleRobot(subtask, robots) && m.SystemConfig.TrafficCfg.UseIdleAvoid {
			// 	if record, find := startWaitTime[robot.Account.AgvId]; !find || (find && record.taskId != subtask.Id) {
			// 		startWaitTime[robot.Account.AgvId] = taskRecord{taskId: subtask.Id, startTime: time.Now()}
			// 	}
			// 	if time.Now().Sub(startWaitTime[robot.Account.AgvId].startTime) < time.Duration(m.SystemConfig.TrafficCfg.TaskAvoidCfg.WaitIdleAgvTime)*time.Second {
			// 		continue
			// 	}
			// 	log.Infof("等待时间超过%fs", m.SystemConfig.TrafficCfg.TaskAvoidCfg.WaitIdleAgvTime)
			// }

			if mainTask == nil || mainTask.TaskId == 0 || mainTask.TaskId == subtask.TaskId {
				node := m.mc.TRM.GetNode(subtask.MapName, subtask.TargetSiteID)
				//调用电梯接口发送到中间件
				if subtask.SwitchType == model.FactoryRealtion || subtask.SwitchType == model.FloorRelation {
					if subtask.SwitchType == model.FloorRelation && subtask.SwitchDevice != "" {
						//当小车失去定位，并且处于自动状态
						// if robot.Status.SiteIdS == "0" && robot.Status.ControlType == 1 {
						// 	//查找上条子任务的目标点，
						// 	subtask.Index

						// }
						if robot.Status.SiteIdS == subtask.SwitchFromSite && robot.Status.MapName == subtask.SwitchFromMap {
							//判断当前小车的确在上一个任务的电梯点
							// if robot.Status.MapName == subtask.SwitchFromMap {
							log.Warnf("robot:%s 通过设备%s 切换楼层到:%s", subtask.AgvId, subtask.SwitchDevice, subtask.MapName)
							// err := m.mc.DM.SwitchFloor(subtask.SwitchDevice, subtask.AgvId, subtask.MapName)
							err := m.mc.DM.SwitchFloor(subtask.SwitchDevice, subtask.AgvId, subtask.MapName, subtask.TaskId)
							if err != nil {
								continue
							}
							log.Infof("robot:%s 切换楼层到:%s 成功", subtask.AgvId, subtask.MapName)
							m.mc.TM.UpdateSubtaskKeyStringToDB(ctx, subtask.Id, "switch_device", "")
						} else {
							continue
						}
					}
					if robot.Status.MapName != subtask.MapName {
						caller, find := m.callers[subtask.AgvId]
						if !find {
							log.Errorf("robot caller not found:%s", subtask.AgvId)
							continue
						}
						robotmanager.SetMapNameToDB(ctx, subtask.AgvId, subtask.MapName)
						map_info := m.mc.TRM.GetMapFF(subtask.MapName)
						if errCall := caller.SwitchMap(ctx, map_info.ProjectId, map_info.TrafficName, subtask.SwitchToSite, subtask.SwitchToPath); errCall != nil {
							continue
						}
						log.Infof("切换AGV%s 地图到%s", subtask.AgvId, subtask.MapName)
						continue
					}
				}
				if subtask.NextSwitchType == model.FloorRelation && subtask.NextSwitchDevice != "" {
					//呼叫电梯前要先占用电梯
					if node.Type == model.NodeElevator {
						currentElevetor := m.DM().GetElevatorInCache(ctx, node.DeviceID)
						if currentElevetor != nil && currentElevetor.AgvId != "" && currentElevetor.AgvId != subtask.AgvId {
							continue
						}
						if currentElevetor != nil && currentElevetor.AgvId == "" {
							m.DM().ModifyElevatorOccupyToDB(ctx, node.DeviceID, subtask.AgvId)
							continue
						}
					}
					log.Warnf("robot:%s 提前呼叫电梯%s 楼层到:%s", subtask.AgvId, subtask.NextSwitchDevice, subtask.MapName)
					// err := m.mc.DM.SwitchFloor(subtask.NextSwitchDevice, subtask.AgvId, subtask.MapName)
					err := m.mc.DM.SwitchFloor(subtask.NextSwitchDevice, subtask.AgvId, subtask.MapName, subtask.TaskId)
					if err == codes.ErrDeviceNotEnable || err == codes.ErrDeviceNotOnline {
						continue
					}
					m.mc.TM.UpdateSubtaskKeyStringToDB(ctx, subtask.Id, "next_switch_device", "")
				}
				//判断是否需要请求目标设备
				if subtask.IsCallDevice && subtask.Type == model.TaskTypePick {
					//告诉上层系统称重
					if node.Type == model.NodePlatformWeight && node.DeviceID != "" {
						//开始称重
						err := m.mc.DM.OperatorAll(context.Background(), node, subtask.AgvId, subtask.MapName, model.OperatorWeighPick)
						if err != nil {
							log.Errorf("取货称重失败,错误信息：%s !!!", err.Error())
							continue
						}
						log.Infof("任务编号：%d,取货称重成功 !!!", subtask.TaskId)
					}
				}
				//目标点
				// if subtask.NextSwitchType == model.FactoryRealtion && node.Type == model.NodeSwitch {
				// 	//提前占用厂区切换点
				// 	err := m.DM().ModifyAreaSwitchOccupyToDB(ctx, subtask.MapName, node.Id, subtask.AgvId)
				// 	if err != nil {
				// 		continue
				// 	}
				// }

				if subtask.SubStatus != model.InitState {
					m.mc.TM.UpdateSubtaskKeyToDB(ctx, subtask.Id, "sub_status", model.InitState)
				}
				m.mc.TM.UpdateSubtaskKeyIntToDB(context.Background(), subtask.Id, "retry_count", subtask.RetryCount+1)
				log.Warnf("已解析的任务开始下发处理，subtask:%d rmsTaskId:%d", subtask.Id, subtask.TaskId)
				if errRun := m.RunSubtask(context.Background(), subtask, robot, false, m.SystemConfig.TrafficCfg.UseTaskAvoid); errRun != nil {
					if subtask.RetryCount%15 == 0 {
						log.Errorf("run robot:%s subtask:%d error:%s", robot.Account.AgvId, subtask.Id, errRun.Error())
					}
					if subtask.Type == model.TaskTypeAvoid && subtask.RetryCount > 3 {
						ctxTx := boot.Begin(ctx)
						m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, subtask.Id, model.TaskStatusCanceled, false)
						ctxTx.Commit()
					}
					continue
				}

				// if node.Type == model.NodeElevator && node.DeviceID != "" {
				// 	m.DM().ModifyElevatorOccupyToDB(ctx, node.DeviceID, subtask.AgvId)
				// } else
				if node.Type == model.NodeSwitch {
					m.DM().ModifyAreaSwitchOccupyToDB(ctx, subtask.MapName, node.Id, subtask.AgvId)
				}
			}
		}
	}

}

//执行中的任务
func (m *DispatchManager) CheckRunningSubtask() {
	ctx := context.Background()
	for {
		tools.Sleep(3)
		//获取所有AGV
		robots := robotmanager.GetAllRobotsFromCache()
		if len(robots) <= 0 {
			continue
		}
		for _, robot := range robots {
			if !robot.StatusIsExists() || !robot.Status.TaskOnline || !robot.Status.Online {
				continue
			}
			//检测小车存在未完成的子任务不可下发子任务
			runTask, _ := m.mc.TM.GetRunningSubtaskFromDB(ctx, robot.Account.AgvId)
			if runTask == nil {
				continue
			}
			records, errGet := m.mc.TRM.GetAllFailedPlanToDB(ctx)
			if errGet != nil {
				continue
			}
			inFailedRecord := false
			for _, record := range records {
				if record.AgvID == robot.Account.AgvId {
					inFailedRecord = true
					break
				}
			}
			if inFailedRecord {
				continue
			}
			planer := m.TRM().GetPathPlanner(robot.Status.MapName)
			if planer == nil {
				continue
			}
			globalRejects, globalRejectsAll, localRejects := m.mc.MakePlanRejectNodes(robot.Account.AgvId, runTask.MapName, robot.AgvCorverNodes, runTask)
			targetNode := planer.GetNode(runTask.TargetSiteID)

			if runTask.BlockAgv != "" {
				blockRobot := robotmanager.GetFromCache(runTask.BlockAgv)
				if blockRobot.Status != nil {
					if runTask.BlockAgvSite == blockRobot.Status.SiteIdS {
						// log.Warnf("子任务:%d, blockRobot=%s未挪动，不予打断任务", runTask.Id, blockRobot.Account.AgvId)
						continue
					} else {
						//更新阻挡该任务执行的agv的id为空
						m.mc.TM.UpdateSubtaskBlockInfoToDB(ctx, runTask.Id, "", "0", 0, 0, time.Now(), 0)
					}
				}

			}
			if robot.StatusIsExists() && (m.RobotGpathsLess(ctx, robot.Account.AgvId) || robot.Status.SiteIdS == runTask.TempTargetSite) {
				continue
			}
			taskNeedClear := false
			switch runTask.SubStatus {
			case model.WalkToTemp, model.WalkToTargetReplace, model.WalkToTempReplace:
				if _, _, errPlan, _ := m.mc.TRM.PlanOnce(ctx, robot, runTask.MapName, robot.Status.SiteIdS, runTask.TargetSiteID, runTask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, false, false, true, false); errPlan == nil {
					taskNeedClear = true
				}
			case model.WalkNearAvoid, model.WalkAvoidFinish:
				if _, _, errPlan, _ := m.mc.TRM.PlanOnce(ctx, robot, runTask.MapName, robot.Status.SiteIdS, runTask.TargetSiteID, runTask.TargetSiteID, globalRejects, globalRejectsAll, localRejects, false, false, true, false); errPlan == nil {
					taskNeedClear = true
				} else if targetNode.IntermediateNode != "0" && targetNode.IntermediateNode != "" {
					if _, _, errPlan, _ := m.mc.TRM.PlanOnce(ctx, robot, runTask.MapName, robot.Status.SiteIdS, runTask.TargetSiteID, targetNode.IntermediateNode, globalRejects, globalRejectsAll, localRejects, false, false, true, false); errPlan == nil {
						taskNeedClear = true
					}
				}
			}

			if taskNeedClear {
				m.normalTaskLock[robot.Account.MapName].Lock()
				err := m.TRM().DeleteRobotGlobalPaths(ctx, robot.Account.AgvId, robot.Account.MapName)
				if err != nil {
					continue
				}
				log.Infof("清除robot:%s全局路径", robot.Account.AgvId)
				m.normalTaskLock[robot.Account.MapName].Unlock()
				//下发清除指令到AGV
				caller, _ := m.GetCaller(runTask.AgvId)
				if caller == nil {
					log.Infof("没有找到小车连接")
					continue
				}
				if err := caller.OperateTask(ctx, runTask.Id, m.mc.TM, proto.TaskOperatePb_TaskClear); err == nil {
					m.mc.TM.UpdateSubtaskKeyToDB(ctx, runTask.Id, "sub_status", model.WalkTaskClearing)
					m.mc.TRM.AddFailedPlanToDB(ctx, robot.Account.AgvId, "", robot.Account.MapName, model.PathPlanFailed) //任务扔到FailedPlan
					log.Warnf("subtask:%d task:%d robot:%s 全局检测可达，清空任务到AGV，任务扔到FailedPlan", runTask.Id, runTask.TaskId, runTask.AgvId)
				}
			}
			if runTask.TargetSiteID == robot.Status.SiteIdS {
				// log.Warnf("AGV%s 当前在目标点", runTask.AgvId)
				continue
			}
			noPath := m.mc.TRM.IsAgvNoLocalPath(runTask.MapName, runTask.AgvId)

			if !inFailedRecord && noPath && time.Since(runTask.BlockAvoidTime) > 60*time.Second && runTask.Status != model.TaskStatusCharging {
				m.mc.TM.UpdateSubtaskKeyToDB(ctx, runTask.Id, "sub_status", model.InitState)
				m.mc.TRM.AddFailedPlanToDB(ctx, runTask.AgvId, "", runTask.MapName, model.PathPlanFailed)
				log.Warnf("AGV%s inFailedRecord,任务扔到FailedPlan", runTask.AgvId)
			}
		}
	}
}

//任务状态改为完成
func (m *DispatchManager) TaskFinish() {
	ctx := context.Background()
	for {
		tools.Sleep(0.2)
		tasks, err := m.TM().GetAllStatusTasksFromDB(ctx, model.TaskRunningStatus())
		if err != nil {
			log.Errorf("任务完成处理失败，获取任务失败")
			continue
		}
		for _, task := range tasks {
			subtasks, err := m.TM().GetAllSubtasksByTaskIdFromDB(ctx, task.Id)
			if err != nil {
				log.Errorf("任务完成处理失败，获取任务失败")
				continue
			}
			isFinish := true
			for _, subtask := range subtasks {
				if subtask.Status.IsRunning() {
					isFinish = false
					break
				}
				if subtask.IsCompleteProcess == 0 {
					isFinish = false
					break
				}
			}

			if !isFinish {
				continue
			}

			ctxTx := boot.Begin(ctx)
			//任务状态改为完成
			err = m.TM().UpdateTaskStatusToDB(ctxTx, task.Id, model.TaskStatusFinish)
			if err != nil {
				ctxTx.Rollback()
				log.Errorf("任务完成失败，更改任务(%d)状态失败", task.Id)
				continue
			}
			//解除任务绑定
			if task.MainTaskCode == "" {
				err = m.TM().DeleteMainTaskByAgvIdToDB(ctxTx, task.AgvId)
				if err != nil {
					ctxTx.Rollback()
					log.Errorf("任务完成失败,数据库解除任务绑定失败  taskid:%d", task.Id)
					continue
				}
			} else {
				err = m.TM().UpdateMainTaskIdToDB(ctxTx, task.MainTaskCode, 0, task.AgvId)
				if err != nil {
					ctxTx.Rollback()
					log.Errorf("任务完成失败,数据库解除任务绑定失败  taskid:%d", task.Id)
					continue
				}
			}
			//删除对应子任务
			err = m.TM().DeleteSubtasksToDB(ctxTx, task.Id)
			if err != nil {
				ctxTx.Rollback()
				log.Errorf("任务完成失败，数据库删除子任务失败  taskid:%d", task.Id)
				continue
			}
			//更新AGVCount状态
			//查询状态更新有货标记，先获得AGV驱动
			robot := robotmanager.GetFromCache(task.AgvId)
			if robot.Account.AgvIP != "127.0.0.1" {
				caller, _ := m.GetCaller(task.AgvId)
				if caller == nil {
					ctxTx.Rollback()
					log.Errorf("can not find robot caller")
					continue
				}
				//读取货物状态
				sensorData, err := caller.GetSensorData(ctxTx, 1)
				if err != nil || sensorData == nil {
					log.ErrorfWithContext(ctx, "主动获取设备读取的数据错误 error:%s", err.UnWrapError())
					continue
				}
				goodStatus_str := sensorData.GetSensorData()
				// var goods_status int64
				goods_status, err1 := strconv.ParseInt(goodStatus_str, 10, 32)
				if err1 != nil {
					ctxTx.Rollback()
					log.Errorf("任务完成失败，获得传感器有货数据:%s  转换成int32失败:%s！！！", goodStatus_str, err1.Error())
					continue
				}
				err = robotmanager.SetGoodsStatusToDB(ctxTx, task, int32(goods_status))
				if err != nil {
					ctxTx.Rollback()
					log.Errorf("任务完成失败，保存AGV货物状态到数据库失败  taskid:%d", task.Id)
					continue
				}
			}
			//更新轮廓
			if robot.StatusIsExists() {
				err1 := m.TRM().LockNodeAround(ctxTx, robot, robot.Status.MapName, robot.Status.SiteIdS, robot.Status.Angle, robot.Type, false, false)
				if err1 != nil {
					ctxTx.Rollback()
					log.Errorf("任务完成失败taskid:%d ，更新车体轮廓失败 ", task.Id)
					continue
				}
			}

			ctxTx.Commit()
		}
	}
}

//执行子任务的完成处理
func (m *DispatchManager) SubtaskFinishProcess() {
	ctx := context.Background()
	for {
		tools.Sleep(0.2)
		//
		subtasks, err := m.TM().GetAllFinishProcessSubtasksFromDB(ctx)
		if err != nil {
			log.Errorf("执行子任务的完成处理失败，获取任务失败")
			continue
		}
		for _, subtask := range subtasks {
			err = m.TM().UpdateSubtaskCompletProcessToDB(ctx, subtask.Id, 1)
			if err != nil {
				log.Errorf("执行子任务(%d)的完成处理失败，修改任务完成处理标志失败", subtask.Id)
				continue
			}
		}
	}
}
func (m *DispatchManager) PathPlanNear(ctx context.Context, robot *model.Robot, subtask *model.Subtask, blockAgv string, blockPath model.TrafficPaths) string {
	robotInReject := func(robot *model.Robot, rejects map[string]string) bool {
		for _, site := range robot.AgvCorverNodes {
			if _, find := rejects[site]; find {
				return true
			}
		}
		return false
	}
	if blockPath != nil && len(blockPath.ToIDs()) > 0 {
		goalRejects := make(map[string]string, 0)
		for _, s := range blockPath.AllBlocks().ToIds() {
			goalRejects[s] = s
		}
		allApprovedLocalPaths, _ := m.TRM().GetPathPlanner(robot.Account.MapName).GetAllLocalPathFromDB(ctx)
		for _, p := range blockPath {
			for _, approved := range allApprovedLocalPaths {
				if approved.Sn == blockAgv || approved.Paths != nil {
					continue
				}
				isConflict, _ := approved.Conflict(p, m.TRM().GetPathPlanner(subtask.MapName).GetConnectPaths, false, robot)
				if !isConflict {
					//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
					isConflict, _ = approved.RotateConflictRect(p, robot)
				}
				if isConflict {
					for _, site := range approved.SpecialLocks.ToIds() {
						goalRejects[site] = site
					}
				}
			}
		}
		if robotInReject(robot, goalRejects) {
			globalRejects, globalRejectsAll, localRejects, freeAvoidRejects := m.mc.MakePlanRejectNodes1(robot.Account.AgvId, robot.Account.MapName, robot.AgvCorverNodes, robot.Status.SiteIdS)
			robots := robotmanager.GetAllRobotsInSameMapFromCache(robot.Status.MapName)
			curRobotSite := ""
			if curRobot := robotmanager.GetFromCache(blockAgv); curRobot.StatusIsExists() {
				curRobotSite = curRobot.Status.SiteIdS
			}
			siteId := m.mc.TRM.GetIdleAgvNearAvoid(context.Background(), robot, robots, curRobotSite, globalRejects, globalRejectsAll, localRejects, goalRejects, freeAvoidRejects, blockPath)
			// if siteId.Id != "" {
			// if gPaths, lPaths, errPlan, _ := m.mc.TRM.PlanOnce(ctx, robot, subtask.MapName, robot.Status.SiteIdS, subtask.TargetSiteID, siteId.Id, globalRejects, globalRejectsAll, localRejects, true, false, false, true); errPlan == nil {
			// 	log.Warnf("robot%s 到站点%s避让-占用目标点%s（触发robot:%s）", robot.Account.AgvId, siteId.Id, robot.Status.SiteIdS, blockAgv)
			// 	return gPaths, lPaths, model.WalkNearAvoid, "0", -1, nil, blockAgv
			// }
			// }
			return siteId.Id
		}
	}
	return ""
}
func (m *DispatchManager) handleTaskAvoidPoint(ctx context.Context, subtask *model.Subtask, robot *model.Robot, wg *sync.WaitGroup) {
	defer wg.Done()
	planer := m.TRM().GetPathPlanner(robot.Status.MapName)
	if planer == nil {
		return
	}
	siteId := ""
	if gPath, _ := planer.GetRobotGlobalPaths(context.Background(), subtask.BlockAgv); gPath != nil && gPath.Paths != nil {
		siteId = m.PathPlanNear(ctx, robot, subtask, subtask.BlockAgv, gPath.Paths)
	} else if runSubtask, err := m.TM().GetRunningSubtaskFromDB(ctx, subtask.BlockAgv); err == nil && runSubtask.Paths != nil {
		siteId = m.PathPlanNear(ctx, robot, subtask, subtask.BlockAgv, runSubtask.Paths)
	}
	if siteId == "" {
		if findBlock, agvs := m.TRM().GetFailedPlanByBlockAgv(context.Background(), robot.Account.AgvId); findBlock {
			for _, a := range agvs {
				if runSubtask, err := m.TM().GetRunningSubtaskFromDB(ctx, a); err == nil && runSubtask.Paths != nil {
					siteId = m.PathPlanNear(ctx, robot, subtask, a, runSubtask.Paths)
					if siteId != "" {
						break
					}
				}
			}
		}
	}
	if siteId != "" {
		m.mc.TM.UpdateSubtaskKeyStringToDB(context.Background(), subtask.Id, "avoid_site", siteId)
	}
	return
}

//子任务下发（retry：是否为失败任务重发）
func (m *DispatchManager) RunSubtask(ctx context.Context, subtask *model.Subtask, robot *model.Robot, retry, avoidFlag bool) codes.Code {
	pathExist := func(p string, list []string) bool {
		for _, l := range list {
			if p == l {
				return true
			}
		}
		return false
	}
	if m.SystemConfig.IsSoftStop {
		return codes.ErrRobotStatusNotMatch.WithDetails("软件急停状态不可下发任务")
	}
	if m.UM().IsAuthorLock() {
		return codes.ErrRobotStatusNotMatch.WithDetails("rms lock")
	}
	if _, find := m.normalTaskLock[subtask.MapName]; !find {
		m.normalTaskLock[subtask.MapName] = new(sync.Mutex)
	}
	if _, find := m.normalTaskDoingFlag[subtask.MapName]; !find {
		m.normalTaskDoingFlag[subtask.MapName] = new(model.NomalTaskOccupy)
	}
	if !robot.StatusIsExists() {
		return codes.ErrDeviceStatusNotExists
	}
	globalRejects, globalRejectsAll, localRejects := m.mc.MakePlanRejectNodes(robot.Account.AgvId, subtask.MapName, robot.AgvCorverNodes, subtask)
	newStatus := model.TaskStatusRunning
	subtaskStatus := subtask.SubStatus
	if subtaskStatus == model.WalkTaskClearing {
		return codes.ErrRobotStatusNotMatch
	}
	planer := m.TRM().GetPathPlanner(robot.Status.MapName)
	if planer == nil {
		return codes.ErrDeviceNotExists
	}
	sourceNode := planer.GetNode(robot.Status.SiteIdS)
	if sourceNode.AllowAvoid == 1 && retry {
		avoidFlag = false
	}
	if retry && avoidFlag && subtask.BlockAgv != "" && subtask.Paths != nil {
		planer.SetHeatingPathID(60, subtask.AgvId, subtask.BlockAgv, []string{subtask.Paths.Front().PathId})
	}
	//判断当前占用对象是否为空
	m.normalTaskLock[subtask.MapName].Lock()
	// log.Warnf("RunSubtaskNew robot:%s 开始下发任务", robot.Account.AgvId)
	m.normalTaskDoingFlag[subtask.MapName].IsOccupy(true)
	defer func() {
		m.normalTaskDoingFlag[subtask.MapName].IsOccupy(false)
		m.normalTaskLock[subtask.MapName].Unlock()
	}()
	//开启事务
	ctxTx := boot.Begin(ctx)
	// 初始状态，避障控制结束或者避让清除完成，尝试重新计算目标点
	gPaths, lPaths, planStatus, tmpTarget, tmpTargetLayer, errGPlan, blockAgv, _ := m.mc.TRM.PathPlan(ctxTx, robot, subtask, globalRejects, globalRejectsAll, localRejects, true, avoidFlag, true, retry)
	defer m.WriteConflictAgv(context.Background(), subtask, blockAgv, errGPlan != nil, retry, gPaths)

	if subtask.BlockAgv != "" && subtask.Paths != nil && gPaths != nil && gPaths.Paths != nil {
		if blockRobot := robotmanager.GetFromCache(subtask.BlockAgv); blockRobot.Status != nil {
			if subtask.BlockAgvSite == blockRobot.Status.SiteIdS && pathExist(subtask.Paths.Front().PathId, gPaths.Paths.ToIDs()) &&
				(lPaths == nil || lPaths.Paths == nil || (lPaths != nil && lPaths.Paths != nil && !pathExist(subtask.Paths.Front().PathId, lPaths.Paths.ToIDs()))) {
				errGPlan = codes.ErrTargerNotReachable
			} else if subtask.BlockAgv == blockAgv {
				errGPlan = codes.ErrTargerNotReachable
			}
		}
	}
	if errGPlan != nil {
		if !avoidFlag || errGPlan == codes.ErrCallDevice {
			if time.Now().Sub(subtask.BlockAvoidTime) > 60*time.Second && subtask.Status == model.TaskStatusRunning && subtask.BlockAgv != "" {
				subtask.SubStatus = model.WalkNearAvoid
				subtask.Status = model.TaskStasusAvoid
				m.mc.TM.UpdateSubtaskToDB(ctxTx, subtask)
				m.mc.TM.UpdateTaskStatusToDB(ctxTx, subtask.TaskId, model.TaskStasusAvoid)
			}
			ctxTx.Rollback()
			return errGPlan
		}
		if subtaskStatus == model.WalkTempFinish && (errors.Is(errGPlan, codes.ErrAlreadyInTransmitPoint)) {
			ctxTx.Rollback()
			return errGPlan
		}
		if subtask.Type == model.TaskTypeAvoid {
			ctxTx.Rollback()
			return errGPlan
		}
		if (subtaskStatus == model.WalkAvoidFinish || subtaskStatus == model.WalkTempFinish) && time.Now().Sub(subtask.BlockAvoidTime) < time.Duration(m.SystemConfig.TrafficCfg.TaskAvoidCfg.AvoidTriggerTime)*time.Second {
			ctxTx.Rollback()
			return errGPlan
		}
		//数值越大，优先级越高，当前任务优先级别低的去避让
		if (subtask.Grade <= int(subtask.BlockTaskGrade+subtask.ExternalGrade) && (subtask.BlockAgv != "" || subtask.BlockTaskGrade != 0)) || subtask.TempTargetSite == "0" {
			// log.Warnf("RmsTask:%d, subtask:%d 尝试局部避障计算", subtask.TaskId, subtask.Id)
			var blockStatus model.TaskSubStatus
			var errGPlan0 codes.Code
			var gPathsTmp *model.GlobalPath
			var lPathsTmp *model.LocalPath
			var blockAgvTmp string
			if subtask.Paths != nil {
				planer.SetHeatingPathID(60, subtask.AgvId, subtask.BlockAgv, []string{subtask.Paths.Front().PathId})
			}
			nearCompute := false
			if subtask.AvoidSite != "" {
				if gPathsTmp, lPathsTmp, errGPlan0, blockAgvTmp = m.mc.TRM.PlanOnce(ctxTx, robot, subtask.MapName, robot.Status.SiteIdS, subtask.TargetSiteID, subtask.AvoidSite, globalRejects, globalRejectsAll, localRejects, true, false, true, true); errGPlan0 == nil {
					log.Warnf("robot%s 到站点%s避让-占用目标点%s（触发robot:%s）", robot.Account.AgvId, subtask.AvoidSite, robot.Status.SiteIdS, subtask.BlockAgv)
					blockStatus = model.WalkNearAvoid
					tmpTarget = "0"
					tmpTargetLayer = -1
					nearCompute = true
				}
			}
			if !nearCompute || errGPlan0 != nil {
				gPathsTmp, lPathsTmp, blockStatus, tmpTarget, tmpTargetLayer, errGPlan0, blockAgvTmp = m.mc.TRM.PathPlanBlock(ctxTx, robot, subtask, globalRejects, globalRejectsAll, localRejects)
			}
			if errGPlan0 != nil {
				if time.Now().Sub(subtask.BlockAvoidTime) > 60*time.Second && subtask.Status == model.TaskStatusRunning && subtask.BlockAgv != "" {
					subtask.SubStatus = model.WalkNearAvoid
					subtask.Status = model.TaskStasusAvoid
					m.mc.TM.UpdateSubtaskToDB(ctxTx, subtask)
					m.mc.TM.UpdateTaskStatusToDB(ctxTx, subtask.TaskId, model.TaskStasusAvoid)
				}
				// log.ErrorfWithContext(ctxTx, "RmsTask:%d, subtask:%d, run task error:%s", subtask.TaskId, subtask.Id, errGPlan.UnWrapError())
				ctxTx.Rollback()
				return errGPlan
			}
			gPaths = gPathsTmp
			lPaths = lPathsTmp
			blockAgv = blockAgvTmp
			if blockStatus == model.WalkForwardAvoid {
				subtaskStatus = model.WalkForwardAvoid
				newStatus = model.TaskStasusAvoid
			} else if blockStatus == model.WalkNearAvoid {
				subtaskStatus = model.WalkNearAvoid
				newStatus = model.TaskStasusAvoid
			} else if blockStatus == model.WalkBackforwardAvoid {
				blockTimeValue := 30 * time.Second //堵塞累计时间阈值，单位秒
				blockedTime := time.Now().Sub(subtask.BlockAvoidTime)
				if blockedTime < blockTimeValue {
					// log.ErrorfWithContext(ctxTx, "RmsTask:%d, subtask:%d, run task error:%s", subtask.TaskId, subtask.Id, errGPlan.UnWrapError())
					ctxTx.Rollback()
					return errGPlan
				}
				subtaskStatus = model.WalkBackforwardAvoid
				newStatus = model.TaskStasusAvoid
			}
			subtask.AvoidSite = tmpTarget
		} else {
			if subtask.BlockAgv != "" {
				if time.Now().Sub(subtask.BlockStartTime) > 120*time.Second {
					externalGrade := subtask.Grade - subtask.BlockTaskGrade + 1
					subtask.ExternalGrade = externalGrade
					m.mc.TM.UpdateSubtaskKeyIntToDB(ctx, subtask.Id, "external_grade", externalGrade)
				}
			}
			ctxTx.Rollback()
			return errGPlan
		}

	} else {
		if planStatus == model.WalkToTarget {
			subtaskStatus = model.WalkToTarget
			node := planer.GetNode(subtask.TargetSiteID)
			if node.Type == model.NodeSwitch {
				m.DM().ModifyAreaSwitchOccupyToDB(ctx, subtask.MapName, subtask.TargetSiteID, subtask.AgvId)
			}
		} else if planStatus == model.WalkToTemp {
			subtaskStatus = model.WalkToTemp
			newStatus = model.TaskStatusInTheTransit
		} else if planStatus == model.WalkToTargetReplace {
			subtaskStatus = model.WalkToTargetReplace
			newStatus = model.TaskStasusAvoid
		} else if planStatus == model.WalkToTempReplace {
			subtaskStatus = model.WalkToTempReplace
			newStatus = model.TaskStasusAvoid
		}
		subtask.ExternalGrade = 0
		subtask.TempTargetSite = tmpTarget
	}

	caller, find := m.callers[robot.Account.AgvId]
	if !find {
		log.Errorf("robot caller not found:%s", robot.Account.AgvId)
		ctxTx.Rollback()
		return codes.ErrCallService.WithDetails("robot caller not found")
	}

	subtask.SubStatus = subtaskStatus

	//避让点，替换位对应货架的层数
	if subtaskStatus == model.WalkToTargetReplace || subtaskStatus == model.WalkToTempReplace || subtaskStatus == model.WalkNearAvoid {
		if tmpTargetLayer != -1 {
			subtask.TargetLayer = int32(tmpTargetLayer)
			if robot.Status.GoodsStatus == 1 || robot.Status.GoodsStatus == 2 {
				subtask.WalkType = 1
			} else {
				subtask.WalkType = 2
			}
		}
	} else if subtaskStatus == model.WalkToTarget {
		subtask.TargetLayer = subtask.TargetLayerCopy
		subtask.WalkType = subtask.WalkTypeCopy
	} else if subtask.WalkType != 0 && (subtaskStatus == model.WalkForwardAvoid || subtaskStatus == model.WalkBackforwardAvoid) {
		subtask.WalkType = 0
	}
	errUpdate := m.mc.TM.UpdateSubtaskToDB(ctxTx, subtask)
	if errUpdate != nil {
		log.Errorf("update subtask:%d err:%s", subtask.Id, errUpdate.Error())
		ctxTx.Rollback()
		return errUpdate
	}
	isTaskStart := !retry
	if errStart := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, subtask.Id, newStatus, isTaskStart); errStart != nil {
		log.Errorf("set subtask:%d start error:%s", subtask.Id, errStart.Error())
		ctxTx.Rollback()
		return codes.ErrTaskStart.Wrap(errStart)
	}
	//不是待命任务，删除绑定的待命点和待命任务
	if subtask.Type != model.TaskTypeStandBy {
		if errDelete := m.mc.TM.DeleteRobotStandByTaskToDB(ctxTx, robot.Account.AgvId); errDelete != nil {
			log.Errorf("delete robot :%s standby task error:%s", robot.Account.AgvId, errDelete.Error())
			ctxTx.Rollback()
			return errDelete
		}

	}
	if errFree := m.mc.TRM.FreeRobotStandByPoint(ctxTx, robot.Account.AgvId); errFree != nil {
		log.Errorf("free robot standby point error:%s", errFree.Error())
		ctxTx.Rollback()
		return errFree
	}
	//待命任务锁定待命点
	if subtask.Type == model.TaskTypeStandBy {
		//+判断是否需要释放
		if errLock := m.mc.TRM.LockRobotStandByPoint(ctxTx, subtask.MapName, subtask.TargetSiteID, robot.Account.AgvId); errLock != nil {
			log.Errorf("lock robot standby point error:%s", errLock.Error())
			ctxTx.Rollback()
			return errLock
		}
	}
	if gPaths == nil && lPaths == nil {
		ctxTx.Rollback()
		return codes.ErrSubtaskError
	}
	//下发任务
	if errCall := caller.RunTask(ctxTx, subtask, gPaths, lPaths, robot, m); errCall != nil {
		log.Error("call robot run task error:", errCall.Error())
		ctxTx.Rollback()
		return codes.ErrCallService.Wrap(errCall)
	}
	if subtask.Type == model.TaskTypeAvoid || subtask.Type == model.TaskTypeStandBy {
		robotmanager.UpdateTaskFlgToCache(robot.Account.AgvId, true)
	}
	log.Warnf("下发任务成功 robot:%s | subtaskid:%d | taskid:%d | retry:%t | 全局路径:%v | 局部路径:%v | walkType:%d | 货叉方向:%d", robot.Account.AgvId, subtask.Id, subtask.TaskId, retry, gPaths.Paths.ToIDs(), lPaths.Paths.ToIDs(), subtask.WalkType, subtask.TargetDirection)

	ctxTx.Commit()
	return nil
}

//向AGV下发断电任务
func (m *DispatchManager) SendDischargeTask(ctx context.Context, agv_id string) codes.Code {
	//开启事务
	ctxTx := boot.Begin(ctx)
	caller, find := m.callers[agv_id]
	if !find {
		log.Errorf("robot caller not found:%s", agv_id)
		ctxTx.Rollback()
		return codes.ErrCallService.WithDetails("robot caller not found")
	}
	//下发任务
	if errCall := caller.DischargeTask(ctxTx); errCall != nil {
		log.Error("call robot run task error:", errCall.Error())
		ctxTx.Rollback()
		return codes.ErrCallService.Wrap(errCall)
	}
	ctxTx.Commit()
	log.Infof("下发断电任务到小车成功 robot:%s", agv_id)
	return nil
}

type taskSolveRes struct {
	robot         *model.Robot        //小车信息
	cost          float64             //代价值
	planStatus    model.TaskSubStatus //是否去中转点
	robotSubtasks []model.Subtask
}

//任务分车列表
type solvedRobots []*taskSolveRes

func (results solvedRobots) Len() int { return len(results) }
func (results solvedRobots) Less(i, j int) bool {
	robotI := results[i]
	robotJ := results[j]
	statusI := robotI.robot.Status.Status
	statusJ := robotJ.robot.Status.Status
	switch {
	case statusI == model.RobotStatusIdle && statusJ == model.RobotStatusCharging:
		return true
	case statusI == model.RobotStatusStandby && statusJ == model.RobotStatusCharging:
		return true
	case statusI == model.RobotStatusExecStandby && statusJ == model.RobotStatusCharging:
		return true
	}
	//全局路径代价值
	//if robotI.planStatus == model.WalkToTarget && robotJ.planStatus != model.WalkToTarget {
	//	return true
	//}
	return robotI.cost < robotJ.cost
}
func (results solvedRobots) Swap(i, j int) { results[i], results[j] = results[j], results[i] }
func (results *solvedRobots) Push(x interface{}) {
	*results = append(*results, x.(*taskSolveRes))
}

func (results *solvedRobots) Pop() interface{} {
	old := *results
	n := len(old)
	x := old[n-1]
	*results = old[0 : n-1]
	return x
}

// 计算执行任务的代价值
func (m *DispatchManager) SubtaskAvailableRobotCheck(ctx context.Context, r *model.Robot, t *model.Subtask) (plan_status model.TaskSubStatus, success bool) {

	//任务指定机器人
	if t.AgvId != "" && t.AgvId != r.Account.AgvId {
		//log.Warn("robot id not match:", r.Account.AgvId)
		return model.InitState, false
	}
	runningTask, err := m.mc.TM.GetRobotRunningTaskFromDB(context.TODO(), r.Account.AgvId)
	if err != nil && !errors.Is(err, codes.ErrTaskNotExists) {
		log.Warnf("check robot:%s running task error::%s", r.Account.AgvId, err.UnWrapError())
		return model.InitState, false
	}
	if runningTask != nil && runningTask.Type != model.TaskTypeStandBy && runningTask.Type != model.TaskTypeCharge && runningTask.Type != model.TaskTypeAvoid {
		log.Warnf("robot:%s is running another task:%d", r.Account.AgvId, runningTask.Id)
		return model.InitState, false
	}

	//机器人所处厂区一致 todo 多楼层
	if !r.StatusIsExists() || r.Status.MapName != t.MapName {
		log.Warn("robot factory or floor not match:", r.Account.AgvId)
		return model.InitState, false
	}

	//距离代价判定
	globalRejects, globalRejectsAll, localRejects := m.mc.MakePlanRejectNodes(r.Account.AgvId, t.MapName, r.AgvCorverNodes, t)
	//全局路径规划process
	_, _, planStatus, _, _, errGPlan, _, _ := m.mc.TRM.PathPlan(ctx, r, t, globalRejects, globalRejectsAll, localRejects, false, m.SystemConfig.TrafficCfg.UseTaskAvoid, false, false)

	if errGPlan != nil {
		log.Error("错误：%s", errGPlan.Error())
		return model.InitState, false
	}
	return planStatus, true
}

//检查机器人充电情况，达到断电条件进行主动断电、收到断电指令
func (m *DispatchManager) checkRobotCharge() {
	go func() {
		for {
			ctx := context.Background()
			dealWithDisCharger := func(find bool, charger *chargemanager.Charger, r *model.Robot) {
				charger.GetOperateLocker().Lock()
				defer charger.GetOperateLocker().Unlock()
				if find && r.Status.Status != model.RobotStatusCharging && r.Status.SiteIdS != charger.GetInfo().SiteId && r.Status.SiteId != 0 {
					//存在断电指令
					log.Infof("检测到小车(%s)有断电指令,当前车不在充电点(非充电状态)，开始断电...", r.Account.AgvId)
					runningSubtask, err := m.mc.TM.GetRunningSubtaskFromDB(ctx, charger.GetInfo().AgvID)
					if err != nil {
						return
					}
					if runningSubtask == nil {
						log.Warnf("获取执行中的任务为空")
						if find {
							delete(m.disChargeList, charger.GetInfo().Id)
						}

						return
					}

					if runningSubtask.Type == model.TaskTypeCharge {
						log.Infof("检测到小车(%s)有断电指令，开始断电任务...", r.Account.AgvId)
						ctxTx := boot.Begin(ctx)
						if err := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningSubtask.Id, model.TaskStatusCanceling, false); err != nil {
							ctxTx.Rollback()
							return
						}
						//取消任务
						caller, _ := m.GetCaller(runningSubtask.AgvId)
						if caller == nil {
							ctxTx.Rollback()
							// err := codes.ErrRobotOperate.WithDetails("can not find robot caller")
							return
						}
						if err := caller.OperateTask(ctxTx, runningSubtask.Id, m.mc.TM, proto.TaskOperatePb_TaskCancel); err != nil {
							log.ErrorfWithContext(ctxTx, "operate task error:%s", err.UnWrapError())
							ctxTx.Rollback()
							return
						}
						log.Infof("检测到小车(%s)有断电指令，开始释放机器人占用的充电桩...", r.Account.AgvId)
						if errFinish := m.mc.CM.FinishCharger(ctx, runningSubtask.TaskId, runningSubtask.AgvId); errFinish != nil {
							log.Errorf("finish robot:%s charger error:%s", r.Account.AgvId, errFinish.UnWrapError())
							ctxTx.Rollback()
							return
						}
						ctxTx.Commit()
					}
					if find {
						delete(m.disChargeList, charger.GetInfo().Id)
					}
					log.Infof("机器人:%s,还未充电断电指令完成，电量:%d,满电阈值:%d", r.Account.AgvId, r.Status.Power, r.Type.ChargeToTaskPercent)

				} else if (find && r.Status.Status == model.RobotStatusCharging) || (r.Status.Status == model.RobotStatusCharging && r.Status.Power >= 99 && charger.GetStatusWord().Current < 1) {
					log.Infof("检测到小车(%s)有断电指令，开始断电...(电池电量：%d 发现断电指令：%t)", r.Account.AgvId, r.Status.Power, find)
					runningSubtask, err := m.mc.TM.GetRunningSubtaskFromDB(ctx, charger.GetInfo().AgvID)
					if err != nil {
						return
					}
					if runningSubtask == nil {
						log.Warnf("获取执行中的任务为空")
						if find {
							delete(m.disChargeList, charger.GetInfo().Id)
						}

						return
					}
					if runningSubtask.Type == model.TaskTypeCharge {
						log.Infof("检测到小车(%s)有断电指令，开始向AGV下发断电任务...", r.Account.AgvId)
						err = m.SendDischargeTask(ctx, r.Account.AgvId)
						if err != nil {
							log.Errorf("send finish to robot:%s charger error:%s", r.Account.AgvId, err.UnWrapError())
							return
						}
						log.Infof("检测到小车(%s)有断电指令，开始断电任务...", r.Account.AgvId)
						err = m.breakTask(runningSubtask, nil)
						if err != nil {
							return
						}
						log.Infof("检测到小车(%s)有断电指令，开始释放机器人占用的充电桩...", r.Account.AgvId)
						if errFinish := m.mc.CM.FinishCharger(ctx, runningSubtask.TaskId, runningSubtask.AgvId); errFinish != nil {
							log.Errorf("finish robot:%s charger error:%s", r.Account.AgvId, errFinish.UnWrapError())
							return
						}
					}
					if find {
						delete(m.disChargeList, charger.GetInfo().Id)
					}
					log.Infof("机器人:%s,完成充电，电量:%d,满电阈值:%d", r.Account.AgvId, r.Status.Power, r.Type.ChargeToTaskPercent)
				}
			}
			//获取所有充电桩
			chargers := m.mc.CM.GetAllChargersFromCache(ctx)
			for _, charger := range chargers {
				if charger.GetInfo().AgvID != "" {
					//获取充电桩锁定的机器人
					r := robotmanager.GetFromCache(charger.GetInfo().AgvID)
					if !r.StatusIsExists() {
						continue
					}
					// m.disChargeLock.Lock()
					_, find := m.disChargeList[charger.GetInfo().Id]
					dealWithDisCharger(find, charger, r)
					// m.disChargeLock.Unlock()

				}
			}
			time.Sleep(3 * time.Second)
		}
	}()
}

//检查失败充电桩，进行处理
func (m *DispatchManager) checkFailedCharger() {
	checkFailed := func(charger *chargemanager.Charger, r *model.Robot) {
		charger.GetOperateLocker().Lock()
		defer charger.GetOperateLocker().Unlock()
		ctx := context.Background()

		//检查任务
		runningSubtask, err := m.mc.TM.GetRunningSubtaskFromDB(ctx, r.Account.AgvId)
		if runningSubtask == nil && r.Status.SiteId != 0 && r.Status.SiteIdS != charger.GetInfo().SiteId {
			//存在未完成的充电任务
			unFinishRechargerTask, err := m.mc.TM.GetUnFinishedRechargerFromDB(ctx, r.Account.AgvId)
			if err != nil {
				log.Errorf("robot:%s，获得未完成的充电任务错误！！！ ", r.Account.AgvId)
				return
			}
			if unFinishRechargerTask == nil {
				log.Infof("robot:%s，当前任务为空，并且不在充电点释放充电桩 ", r.Account.AgvId)
				_ = m.mc.CM.FreeCharger(ctx, charger.GetInfo().MapName, charger.GetInfo().SiteId, charger.GetInfo().Id)
			}
			//释放充电桩
			return
		}
		if err != nil || runningSubtask == nil || runningSubtask.Type != model.TaskTypeCharge || (runningSubtask.Status != model.TaskStatusCharging && runningSubtask.Status != model.TaskStatusError) {
			return
		}
		//++当前充电桩存在断电指令操作，不用做任何操作
		_, findDisCharger := m.disChargeList[charger.GetInfo().Id]
		if findDisCharger {
			return
		}
		//log.Infof("=====> 充电桩电流：%d", charger.GetStatusWord().Current)
		//充电桩电流不为0
		if charger.GetStatusWord().IsOut && charger.GetStatusWord().Current >= 1 {
			return
		}

		//获取充电统计
		statistic, err := m.mc.CM.GetStatisticCharge(ctx, runningSubtask.TaskId, runningSubtask.AgvId)
		if err != nil || statistic == nil {
			//log.Warnf("检查失败充电桩，获取充电统计错误 robot:%s chargerId:%d 错误信息:%s", r.Account.AgvId, charger.GetInfo().Id, err.UnWrapError())
			//操作充电桩开启
			log.Infof("开始控制充电桩伸出(占用robot:%s,状态:%+v)", charger.GetInfo().AgvID, charger.GetStatusWord())
			if errStartCharge := m.mc.CM.StartRobotCharger(ctx, runningSubtask); errStartCharge != nil {
				log.Errorf("start charge error:%s", errStartCharge.Error())
			}
			//等待20秒，用于检查20秒后有没有电流
			tools.Sleep(15)
			return
		}

		if charger.GetStatusWord().IsOut {
			// 无AGV、无电流、充电臂伸出--》清除任务、结束充电
			robot := robotmanager.GetFromCache(runningSubtask.AgvId)
			if !robot.StatusIsExists() {
				log.Errorf("充电异常，获取小车信息失败  robot:%s chargerId:%d", runningSubtask.AgvId, charger.GetInfo().Id)
				return
			}
			currentSiteId := robot.Status.SiteId
			if currentSiteId == 0 {
				return
			}
			if robot.Status.SiteIdS != charger.GetInfo().SiteId && robot.Status.SiteIdS != "0" {
				//车载断继电器
				err = m.SendDischargeTask(ctx, r.Account.AgvId)
				if err != nil {
					log.Errorf("send finish to robot:%s charger error:%s", r.Account.AgvId, err.UnWrapError())
					return
				}
				//清除任务、结束充电
				ctxTx := boot.Begin(ctx)
				//更新任务状态
				err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningSubtask.Id, model.TaskStatusCanceled, false)
				if err != nil {
					ctxTx.Rollback()
					return
				}
				err := m.mc.TM.UpdateTaskStatusToDB(ctxTx, runningSubtask.TaskId, model.TaskStatusCanceled)
				if err != nil {
					ctxTx.Rollback()
					return
				}
				//清除全局路径(只清除路径，不清除锁)
				if errDeletePath := m.mc.TRM.DeleteRobotGlobalPaths(ctxTx, runningSubtask.AgvId, runningSubtask.MapName); errDeletePath != nil {
					ctxTx.Rollback()
					return
				}
				//解除任务绑定
				log.Warnf("robot:%s DeleteMainTaskByAgvIdToDB", runningSubtask.AgvId)
				err = m.mc.TM.DeleteMainTaskByAgvIdToDB(ctxTx, runningSubtask.AgvId)
				if err != nil {
					ctxTx.Rollback()
					return
				}
				//结束充电
				err = m.mc.CM.FinishCharger(ctxTx, runningSubtask.TaskId, robot.Account.AgvId)
				if err != nil {
					ctxTx.Rollback()
					return
				}
				ctxTx.Commit()
			} else if robot.Status.Power < 99 {
				//1、充电桩缩回
				log.Warnf("充电桩伸出，充电电流较小(占用robot:%s,状态:%+v)", charger.GetInfo().AgvID, charger.GetStatusWord())
				// code := m.mc.CM.RetractCharger(charger.GetInfo().Id)
				// if code != nil {
				// 	log.Errorf("检查失败充电桩，充电桩缩回错误 robot:%s chargerId:%d 错误信息:%s", r.Account.AgvId, charger.GetInfo().Id, code.UnWrapError())
				// 	return
				// }
			}
		}
		if charger.GetStatusWord().IsIn {
			//2、AGV在充电桩，解锁充电桩；AGV在充电桩，再伸出充电臂（记录伸出次数、超过一定次数RMS报警）
			if r.Status.SiteIdS == charger.GetInfo().SiteId && r.Status.SiteIdS != "0" {
				//tools.Sleep(5)
				if statistic.ChargeNum < 4 {
					log.Warnf("开始控制充电桩伸出(占用robot:%s,状态:%+v)", charger.GetInfo().AgvID, charger.GetStatusWord())
					code := m.mc.CM.StretchCharger(ctx, runningSubtask.TaskId, runningSubtask.AgvId)
					if code != nil {
						// log.Errorf("检查失败充电桩，robot:%s chargerId:%d 错误信息:%s", r.Account.AgvId, charger.GetInfo().Id, code.UnWrapError())
						return
					}
				} else if runningSubtask.Status != model.TaskStatusError {
					log.Warnf("充电桩伸出次数>3，请检查充电桩和AGV是否存在异常 robot:%s chargerId:%d", r.Account.AgvId, charger.GetInfo().Id)
					ctxTx := boot.Begin(ctx)
					if err := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningSubtask.Id, model.TaskStatusError, false); err != nil {
						ctxTx.Rollback()
					}
					ctxTx.Commit()
				}
				//等待20秒，用于检查20秒后有没有电流
				tools.Sleep(30)
			} else {
				code := m.breakTask(runningSubtask, nil)
				if code != nil {
					log.Errorf("检查失败充电桩，robot:%s chargerId:%d 错误信息:%s", r.Account.AgvId, charger.GetInfo().Id, code.UnWrapError())
					return
				}
			}
		}

	}
	go func() {
		for {
			ctx := context.Background()
			//获取所有充电桩
			chargers := m.mc.CM.GetAllChargersFromCache(ctx)
			for _, charger := range chargers {
				if charger.GetInfo().AgvID == "" {
					continue
				}
				//获取充电桩锁定的机器人
				r := robotmanager.GetFromCache(charger.GetInfo().AgvID)
				if !r.StatusIsExists() {
					continue
				}

				m.manualChargeLock.Lock()
				checkFailed(charger, r)
				m.manualChargeLock.Unlock()
				time.Sleep(100 * time.Millisecond)
			}
			time.Sleep(5 * time.Second)
		}
	}()
}

//生成充电任务
func (m *DispatchManager) genChargeTask() {
	check := func(r *model.Robot, findRule model.DayChargerRule, isFindRule bool) {
		currentRobotId := r.Account.AgvId
		if !r.StatusIsExists() {
			return
		}
		if !r.Account.AutoCharge {
			return
		}
		if !r.Status.Online {
			return
		}
		if r.Status.EmergencyStop == 1 {
			// log.Warn("robot is emergency stop:", r.Account.AgvId)
			return
		}
		if r.Status.ControlType == 0 {
			// log.Warn("robot's control type is manual:", r.Account.AgvId)
			return
		}
		m.manualChargeLock.Lock()
		generateTask := func() {
			var chargerID uint8
			find := false

			chargerID, find = m.manualChargeList[r.Account.AgvId]
			if !find {
				if !isFindRule && r.Status.Power > r.Type.ChargePercent {
					return
				}
				if isFindRule && r.Status.Power > findRule.ChargePercent {
					//在规则内，并且还存在空闲充电桩可以找充电桩
					if !m.mc.CM.IsHaveFreeCharger() || (r.StatusIsExists() && r.Status.Power >= 95) {
						return
					}

				}
			}

			if (isFindRule || !find) && r.Status.Status.In(model.RobotStatusError, model.RobotStatusCharging, model.RobotStatusNavigating, model.RobotStatusPicking, model.RobotStatusPutting) {
				return
			}
			txctx := boot.Begin(context.Background())
			runningTask, errGetTask := m.mc.TM.GetRobotRunningTaskFromDB(txctx, r.Account.AgvId)
			if errGetTask != nil && !errors.Is(errGetTask, codes.ErrTaskNotExists) {
				log.Warnf("check robot running task error:%s", errGetTask.Error())
				txctx.Rollback()
				return
			}
			if !find && runningTask != nil && runningTask.Type != model.TaskTypeStandBy {
				log.Warnf("set robot charge failed,robot:%s is busy", r.Account.AgvId)
				txctx.Rollback()
				return
			}
			//检索是否已存在充电任务
			isAssign, errCheck := m.mc.TM.IsRobotAssignTypeTask(txctx, r.Account.AgvId, model.TaskTypeCharge)
			if errCheck != nil {
				log.Errorf("check robot charge task error:%s", errCheck.UnWrapError())
				txctx.Rollback()
				return
			}
			if isAssign {
				txctx.Rollback()
				delete(m.manualChargeList, r.Account.AgvId)
				return
			}
			//申请充电桩
			elevators := m.DM().GetAllElevatorFromCache()
			robotMaps := []string{}
			if r.Account.GroupID != "" {
				accountGroup, ok := robotmanager.GetRobotGroup(r.Account.GroupID)
				if !ok {
					log.Errorf("get group %s empty", r.Account.GroupID)
					txctx.Rollback()
					delete(m.manualChargeList, r.Account.AgvId)
					return
				}
				if accountGroup.MapList != nil {
					robotMaps = accountGroup.MapList
				}
			} else {
				robotMaps = m.mc.TRM.GetAllMapNames()
			}
			log.Warnf("robot:%s，根据充电指令，开始申请充电桩:%d", r.Account.AgvId, chargerID)
			chargerRule1 := model.ChargeRule{}
			if isFindRule {
				endTime, err := tools.JoinTimeInBeijing(findRule.EndTime)
				if err != nil {
					log.Error("join time err :%s", err.Error())
					return
				}
				chargerRule1.IsUse = isFindRule
				chargerRule1.RuleId = findRule.Id
				chargerRule1.EndTime = endTime

			}
			charger := m.mc.CM.MallocCharger(txctx, r, chargerID, robotMaps, m.mc.TRM, elevators, isFindRule, findRule, chargerRule1)
			if charger != nil {
				log.Warnf("robot:%s，根据充电指令，申请充电桩数据:%v", r.Account.AgvId, charger)
			} else {
				log.Warnf("robot:%s，根据充电指令，申请充电桩数据为空！！！", r.Account.AgvId)
			}

			//申请不到充电桩打断逻辑
			if charger == nil {
				//判断当前车电量小于低电量阈值
				// if r.StatusIsExists() && r.Status.Power <= r.Type.ChargePercent {
				// 	//判断还存在空闲充电桩？
				// 	canUseChargerList := m.mc.CM.GetAllChargersFromCachebyAgvId(r.Account.AgvId)
				// 	//满足条件的充电桩
				//      m.mc.CM.ChargerSelect()

				// }
				//这种情况不需要打断
				if r.StatusIsExists() && r.Status.Power > r.Type.ChargePercent {
					return
				}
				if m.disChargeList != nil {
					chargers := m.mc.CM.GetAllChargersFromCache(txctx)
					for _, my_charger := range chargers {
						agv_id := my_charger.GetInfo().AgvID
						if agv_id != "" {
							robots := robotmanager.GetAllRobotsFromCache()
							for _, r1 := range robots {
								if !r1.StatusIsExists() {
									return
								}
								//占用充电桩的车
								if r1.Account.AgvId == agv_id {
									//电量较小不打断
									// if r1.StatusIsExists() && r1.Status.Power <= r1.Type.ChargePercent {
									// 	break
									// }
									// if my_charger.GetInfo().Rule.IsUse {
									// 	currentBeijinTime := tools.GetCurrentTiemByshanghai()
									// 	ruleEndTime := tools.GetCurrentTiemByshanghai1(my_charger.GetInfo().Rule.EndTime)
									// 	if currentBeijinTime.Sub(ruleEndTime) <= 0 {
									// 		continue
									// 	}
									// }
									if r1.Status.Power > r1.Type.ChargeToTaskPercent {
										if tools.StringSliceIsContain(my_charger.GetInfo().BanAgvList, currentRobotId) {
											break
										}
										//
										m.AddDisCharge(agv_id, my_charger.GetInfo().Id)
										//delete(m.manualChargeList, agv_id)
										log.Infof("Discharge robot%s in charger%d", agv_id, my_charger.GetInfo().Id)
										txctx.Rollback()
										return
									}
								}
							}
						}
					}
				}
				// log.Infof("there is no free charger")
				txctx.Rollback()
				return
			}
			if charger.GetStatusWord().IsOut || charger.GetStatusWord().IsOuting {
				if errStart := m.CM().RetractCharger(charger.GetInfo().Id); errStart != nil {
					log.Warnf("小车开始充电,发送断电信号出错  robot:%s err:%s", r.Account.AgvId, errStart.UnWrapError())
					txctx.Rollback()
					return
				}
			}
			chargerMode := model.CharerModeTypeNotCharger
			var chargerRuleId uint16
			//添加充电任务
			if isFindRule {
				chargerMode = model.CharerModeTypeTimerRule
				chargerRuleId = findRule.Id
			} else {
				chargerMode = model.CharerModeTypeNormal
			}
			t, errAddTask := m.mc.TM.AddToDB(txctx, &model.Task{
				Type:          model.TaskTypeCharge,
				Status:        model.TaskStatusRegister,
				Grade:         99,
				MapName:       charger.GetInfo().MapName,
				AgvId:         r.Account.AgvId,
				TargetSiteID:  charger.GetInfo().SiteId,
				CreatedTime:   time.Now(),
				ChargerRuleId: chargerRuleId,
				ChargerRule:   chargerRule1,
				ChargerMode:   chargerMode,
				GroupList:     []string{r.Account.GroupID},
			})

			if errAddTask != nil {
				log.Errorf("add charge task error:%s", errAddTask.UnWrapError())
				txctx.Rollback()
				return
			}
			delete(m.disChargeList, charger.GetInfo().Id)
			delete(m.manualChargeList, r.Account.AgvId)
			txctx.Commit()
			log.Infof("机器人:%s,电量:%d,状态:%d, 添加充电任务:%d,充电桩:%d", r.Account.AgvId, r.Status.Power, r.Status.Status, t.Id, charger.GetInfo().Id)
		}
		generateTask()
		m.manualChargeLock.Unlock()

	}

	go func() {
		for {
			time.Sleep(3 * time.Second)
			//先判断有没有空闲充电桩
			if !m.mc.CM.IsHaveFreeCharger() {
				continue
			}
			// m.genTaskLock.Lock()
			robots := robotmanager.GetAllExitStatusRobotsFromCache()
			//获得所有存在状态的AGVs
			if robots != nil && len(robots) <= 0 {
				continue
			}
			//将所有存在状态的电量从小到大排序------------
			sort.Slice(robots, func(i, j int) bool {
				// 如果小车状态为nil，将其放到切片的末尾
				if !robots[i].StatusIsExists() {
					return false
				}
				if !robots[j].StatusIsExists() {
					return true
				}
				// 根据City字段排序
				return robots[i].Status.Power < robots[j].Status.Power
			})

			//-------------找出可用的充电规则
			currentTime := tools.GetCurrentTiemByshanghai()
			getDayModeType := WeekdayToChinese(currentTime.Weekday())
			dayRuleList := m.GetAllEnableDayChargerRules()
			//存储可操作的日充电规则
			dealDayChargerRuleList := make([]model.DayChargerRule, 0)
			//判断当前那些规则触发
			for _, rule := range dayRuleList {
				if rule.WeekDayMode != model.WeekDayModeAll && getDayModeType != rule.WeekDayMode {
					//判断当前天是否满足要求,不满足该规则不走
					continue
				}
				isInRange, err := tools.IsInTimeRange(rule.StartTime, rule.EndTime)
				if err != nil {
					log.Errorf("获取IsInTimeRange是否在范围内出错，error:%s", err.Error())
					continue
				}
				if isInRange {
					dealDayChargerRuleList = append(dealDayChargerRuleList, rule)
				}
			}
			//得到，当前每量车的的规则
			ruleDict := m.GetAllEnableDayChargerRuleMap(dealDayChargerRuleList)
			//按照电量从低到高的顺序计算充电-------
			for _, r := range robots {
				var findRule model.DayChargerRule
				isFindRule := false
				if len(ruleDict) > 0 {
					//找出当前车的充电规则
					_, find1 := ruleDict[r.Account.AgvId]
					if find1 {
						isFindRule = true
						findRule = ruleDict[r.Account.AgvId]
					}
				}
				if isFindRule {
					// log.Warnf("robot:%s 开始计算充电，定时充电规则发现:%t,规则详情：%v ", r.Account.AgvId, isFindRule, findRule)
				}
				check(r, findRule, isFindRule)
			}
			// m.genTaskLock.Unlock()
		}
	}()
}

func WeekdayToChinese(weekday time.Weekday) model.DayModeType {
	switch weekday {
	case time.Sunday:
		return model.WeekDayModeSunDay
	case time.Monday:
		return model.WeekDayModeMonday
	case time.Tuesday:
		return model.WeekDayModeTuesday
	case time.Wednesday:
		return model.WeekDayModeWednesday
	case time.Thursday:
		return model.WeekDayModeThursday
	case time.Friday:
		return model.WeekDayModeFriday
	case time.Saturday:
		return model.WeekDayModeSaturday
	}
	return model.WeekDayModeAll
}

//生成待命任务
func (m *DispatchManager) genStandbyTask() {
	lastIdleTimeMap := make(map[string]time.Time, 0)
	ctx := context.Background()
	go func() {
		for {
			// m.genTaskLock.Lock()
			robots := robotmanager.GetAllFromCache()
			for _, r := range robots {
				if !r.StatusIsExists() {
					continue
				}
				if !r.Account.AutoPark {
					delete(lastIdleTimeMap, r.Account.AgvId)
					continue
				}
				if !r.Status.Online {
					continue
				}
				if r.Status.EmergencyStop == 1 {
					log.Warn("robot is emergency stop:", r.Account.AgvId)
					continue
				}
				if r.Status.ControlType == 0 {
					// log.Warn("robot's control type is manual:", r.Account.AgvId)
					continue
				}
				//判断当前小车是否绑定主任务编号
				agvBindTask, err := m.mc.TM.GetMainTaskByAgvFromDB(ctx, r.Account.AgvId)
				if err != nil {
					log.Errorf("数据库查询主任务与小车%s绑定信息错误 error:%s", err.UnWrapError(), r.Account.AgvId)
					continue
				}
				if agvBindTask != nil {
					delete(lastIdleTimeMap, r.Account.AgvId)
					//存在任务绑定不可生成待命任务
					// log.Errorf("小车:%s 存在主任务绑定，不可生成待命任务！！！", r.Account.AgvId)
					continue
				}
				if r.Status.Status == model.RobotStatusIdle {
					last, find := lastIdleTimeMap[r.Account.AgvId]
					if find && time.Now().Sub(last) > time.Duration(m.SystemConfig.TrafficCfg.StandbyTime)*time.Second {
						//检索是否已存在待命任务
						isAssign, errCheck := m.mc.TM.IsRobotAssignTypeTask(ctx, r.Account.AgvId, model.TaskTypeStandBy)
						if errCheck != nil {
							log.Errorf("check robot standby task error:%s", errCheck.UnWrapError())
							continue
						}
						if isAssign {
							delete(lastIdleTimeMap, r.Account.AgvId)
							continue
						}
						//检查小车当前是否存在执行中的任务，存在执行的任务不生成待命任务
						runningTask, err := m.mc.TM.GetRobotRunningTaskFromDB(context.TODO(), r.Account.AgvId)
						if err != nil && !errors.Is(err, codes.ErrTaskNotExists) {
							//log.Warnf("check robot:%s running task error::%s", r.Account.AgvId, err.UnWrapError())
							continue
						}
						if runningTask != nil {
							delete(lastIdleTimeMap, r.Account.AgvId)
							//log.Warnf("robot:%s status is :%d,but can not find robot running task in db", res.robot.Account.AgvId, res.robot.Status.Status)
							continue
						}
						globalRejectsAll, _, _ := m.mc.MakePlanRejectNodes(r.Account.AgvId, r.Account.MapName, r.AgvCorverNodes, nil)
						//查找待命点
						ctxTx := boot.Begin(ctx)
						log.Warnf("robot:%s,开始申请待命点", r.Account.AgvId)
						point := m.mc.TRM.MallocStandByPoint(ctxTx, r, robots, globalRejectsAll, m.mc.TM)
						if point == nil {
							log.Warnf("robot:%s,批复的待命点为空", r.Account.AgvId)
						} else {
							log.Warnf("robot:%s,批复的待命点:%s", r.Account.AgvId, point.SiteId)
						}
						log.Warnf("robot:%s,开始待命点", r.Account.AgvId)
						if point == nil {
							//log.Warnf("机器人:%s待命失败：无空闲待命点", r.Account.AgvId)
							ctxTx.Rollback()
							continue
						}

						//生成待命任务
						t, errAddTask := m.mc.TM.AddToDB(ctxTx, &model.Task{
							Type:         model.TaskTypeStandBy,
							Status:       model.TaskStatusRegister,
							MapName:      point.MapName,
							AgvId:        r.Account.AgvId,
							TargetSiteID: point.SiteId,
							Grade:        1,
							CreatedTime:  time.Now(),
							GroupList:    []string{r.Account.GroupID},
						})
						if errAddTask != nil {
							log.Errorf("add standby task error:%s", errAddTask.UnWrapError())
							ctxTx.Rollback()
							continue
						}
						ctxTx.Commit()
						delete(lastIdleTimeMap, r.Account.AgvId)
						log.Infof("机器人:%s 添加待命任务:%d", r.Account.AgvId, t.Id)
					}
					if !find {
						lastIdleTimeMap[r.Account.AgvId] = time.Now()
					}
					continue
				}
				if r.Status.Status != model.RobotStatusIdle {
					_, find := lastIdleTimeMap[r.Account.AgvId]
					if find {
						delete(lastIdleTimeMap, r.Account.AgvId)
					}
				}
			}
			// m.genTaskLock.Unlock()
			time.Sleep(3 * time.Second)
		}
	}()
}

//打断待命任务和充电任务
func (m *DispatchManager) breakTask(runningTask, newTask *model.Subtask) codes.Code {
	ctx := context.Background()
	switch runningTask.Type {
	case model.TaskTypeAvoid:
		var err codes.Code
		ctxTx := boot.Begin(ctx)
		if runningTask.Status == model.TaskStatusCanceling {
			ctxTx.Commit()
			return nil
		}
		//未解析的任务处理
		if runningTask.Status == model.TaskStatusRegister || runningTask.Status == model.TaskStatusAssign {
			if err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceled, false); err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//已执行的任务处理
		robot := robotmanager.GetFromCache(runningTask.AgvId)
		if robot.StatusIsExists() && robot.Status.Status == model.RobotStatusNavigating {
			m.TRM().DeleteRobotGlobalPaths(ctxTx, robot.Account.AgvId, robot.Account.MapName)
			log.Infof("清除robot:%s全局路径", robot.Account.AgvId)
			//执行任务，打断
			if err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceling, false); err != nil {
				ctxTx.Rollback()
				return err
			}
			//取消任务
			caller, _ := m.GetCaller(runningTask.AgvId)
			if caller == nil {
				ctxTx.Rollback()
				err = codes.ErrRobotOperate.WithDetails("can not find robot caller")
				return err
			}
			if err = caller.OperateTask(ctxTx, runningTask.Id, m.mc.TM, proto.TaskOperatePb_TaskCancel); err != nil {
				log.ErrorfWithContext(ctxTx, "operate task error:%s", err.UnWrapError())
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//已解析的任务处理
		if runningTask.Status == model.TaskStasusResolved || (robot.StatusIsExists() && robot.Status.Status == model.RobotStatusIdle) {
			err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceled, false)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
	case model.TaskTypeStandBy:
		var err codes.Code
		defer func() {
			if err == nil {
				if errFree := m.mc.TRM.FreeRobotStandByPoint(ctx, runningTask.AgvId); errFree != nil {
					log.Errorf("free robot standby point error:%s", errFree.Error())
				}
			}
		}()
		ctxTx := boot.Begin(ctx)
		if runningTask.Status == model.TaskStatusCanceling {
			ctxTx.Commit()
			return nil
		}
		//未解析的任务处理
		if runningTask.Status == model.TaskStatusRegister || runningTask.Status == model.TaskStatusAssign {
			if err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceled, false); err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//已执行的任务处理
		robot := robotmanager.GetFromCache(runningTask.AgvId)
		if robot.StatusIsExists() && (robot.Status.Status == model.RobotStatusNavigating || robot.Status.Status == model.RobotStatusExecStandby) {
			m.TRM().DeleteRobotGlobalPaths(ctxTx, robot.Account.AgvId, robot.Account.MapName)
			log.Infof("清除robot:%s全局路径", robot.Account.AgvId)
			//执行待命任务，打断
			if err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceling, false); err != nil {
				ctxTx.Rollback()
				return err
			}
			//取消任务
			caller, _ := m.GetCaller(runningTask.AgvId)
			if caller == nil {
				ctxTx.Rollback()
				err = codes.ErrRobotOperate.WithDetails("can not find robot caller")
				return err
			}
			if err = caller.OperateTask(ctxTx, runningTask.Id, m.mc.TM, proto.TaskOperatePb_TaskCancel); err != nil {
				log.ErrorfWithContext(ctxTx, "operate task error:%s", err.UnWrapError())
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//已解析的任务处理
		if runningTask.Status == model.TaskStasusResolved || (robot.StatusIsExists() && robot.Status.Status == model.RobotStatusIdle) {
			err = m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceled, false)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}

	//任务状态改为完成  充电桩缩回、解锁充电桩
	case model.TaskTypeCharge:
		if runningTask.Status == model.TaskStatusCanceling {
			return nil
		}
		ctxTx := boot.Begin(ctx)
		//小车有任务，给小车下发取消任务
		robot := robotmanager.GetFromCache(runningTask.AgvId)
		//小车充电中任务处理
		if robot.StatusIsExists() && robot.Status.Status == model.RobotStatusCharging {
			taskStatus := model.TaskStatusCanceled
			if robot.Status.Power > robot.Type.ChargeToTaskPercent {
				taskStatus = model.TaskStatusFinish
			}
			err := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, taskStatus, false)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			//取消任务
			caller, _ := m.GetCaller(runningTask.AgvId)
			if caller == nil {
				ctxTx.Rollback()
				err := codes.ErrRobotOperate.WithDetails("can not find robot caller")
				return err
			}
			err = caller.DischargeTask(ctxTx)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			err = m.mc.CM.FinishCharger(ctxTx, runningTask.TaskId, runningTask.AgvId)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//小车导航中任务处理
		if robot.StatusIsExists() && robot.Status.Status == model.RobotStatusNavigating {
			m.TRM().DeleteRobotGlobalPaths(ctxTx, robot.Account.AgvId, robot.Account.MapName)
			log.Infof("清除robot:%s全局路径", robot.Account.AgvId)
			//执行待命任务，打断
			if err := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceling, false); err != nil {
				ctxTx.Rollback()
				return err
			}
			//取消任务
			caller, _ := m.GetCaller(runningTask.AgvId)
			if caller == nil {
				ctxTx.Rollback()
				err := codes.ErrRobotOperate.WithDetails("can not find robot caller")
				return err
			}
			if err := caller.OperateTask(ctxTx, runningTask.Id, m.mc.TM, proto.TaskOperatePb_TaskCancel); err != nil {
				log.ErrorfWithContext(ctxTx, "operate task error:%s", err.UnWrapError())
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//未解析的任务处理（小车空闲）
		if runningTask.Status == model.TaskStatusRegister || runningTask.Status == model.TaskStatusAssign {
			if err := m.mc.TM.UpdateTaskStatusToDB(ctxTx, runningTask.TaskId, model.TaskStatusCanceled); err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
		//已解析的任务处理（小车空闲）
		if runningTask.Status.IsRunning() && runningTask.Status != model.TaskStatusCanceling && runningTask.Status != model.TaskStatusInFinish {
			//任务状态是充电中，给充电桩下发断电任务
			if runningTask.Status == model.TaskStatusCharging {
				err := m.mc.CM.FinishCharger(ctxTx, runningTask.TaskId, runningTask.AgvId)
				if err != nil {
					ctxTx.Rollback()
					return err
				}
			}
			err := m.mc.TM.UpdateSubtaskStatusToDB(ctxTx, runningTask.Id, model.TaskStatusCanceled, false)
			if err != nil {
				ctxTx.Rollback()
				return err
			}
			ctxTx.Commit()
			return nil
		}
	}
	return codes.ErrTaskStatusNotMatch
}

//通过任务模板生成子任务
func (m *DispatchManager) GetSubTasks(ctx context.Context, task *model.Task, robot *model.Robot) ([]model.Subtask, codes.Code, []int) {
	passIndexArr := make([]int, 0)
	if !robot.StatusIsExists() {
		return nil, codes.ErrDeviceStatusNotExists, passIndexArr
	}
	//1、获取任务模板
	taskTemplate, err := m.TTM().GetTaskTemplate(ctx, robot.Type.CategoryId, task.Type)
	if err != nil {
		log.Errorf("任务模板生成子任务错误，获取任务模板失败  taskId:%d", task.Id)
		return nil, err, passIndexArr
	}
	subtasks := make([]model.Subtask, 0)
	subtask := new(model.Subtask)

	//地堆库任务首先下发到固定的前瞻点 是否是地堆库任务的判断
	if (task.Type == model.TaskTypePut || task.Type == model.TaskTypePick) && (robot.Type.CategoryType == model.CategoryTypeStocker || robot.Type.CategoryType == model.CategoryTypeFronted) {
		if targetNode := m.TRM().GetNode(task.MapName, task.TargetSiteID); targetNode.Type == model.NodeStation && targetNode.LocationType == model.LocationGround { //&& task.IsOrderly != 0
			subtask.ConvertByTask(*task, robot.Account.AgvId)
			groundLocationGroup := m.WMM().FindGroundLocation(task.MapName, task.TargetSiteID)
			if groundLocationGroup != nil {
				subtask.TargetSiteID = groundLocationGroup.CheckPointId
				if task.Type == model.TaskTypePut {
					subtask.TargetSiteID = groundLocationGroup.FrontPointId
				}
				subtask.WalkType = 0
				subtask.WalkTypeCopy = 0
				subtask.Type = model.TaskTypeNavi
				if task.Type == model.TaskTypePick {
					subtask.CheckFlg = 1
				} else {
					// if task.IsOrderly == 2 {
					subtask.CheckFlg = 3
					// } else {
					// 	subtask.CheckFlg = 2
					// }
				}
				subtasks = append(subtasks, *subtask)
			}
		}
	}

	//任务模板为空，正常生成子任务
	if taskTemplate == nil {
		subtask.ConvertByTask(*task, robot.Account.AgvId)
		subtasks = append(subtasks, *subtask)
		return subtasks, nil, passIndexArr
	}

	//2、获取子任务模板
	taskItemTemplates, err := m.TTM().GetTaskItemTemplates(ctx, taskTemplate.Id)
	if err != nil {
		log.Errorf("任务模板生成子任务错误，获取子任务模板失败  taskId:%d", task.Id)
		return nil, err, passIndexArr
	}
	if taskItemTemplates == nil || len(taskItemTemplates) < 1 {
		subtask.ConvertByTask(*task, robot.Account.AgvId)
		subtasks = append(subtasks, *subtask)
		return subtasks, nil, passIndexArr
	}
	//找出最终目标点
	finalTarget := task.TargetSiteID
	//任务类型是称重任务
	if task.Type == model.TaskTypeWeigh {
		//称重任务找出最终目标点，
		if !robot.StatusIsExists() {
			log.Errorf("robot:%s 状态不存在（taskId:%d）", robot.Account.AgvId, task.Id)
			return nil, codes.ErrRobotStatusNotExists, passIndexArr
		}
		robotDeepCopy := robot.GetDeepCopyRobot()
		finalNode, err := m.TRM().MallocWeightPlatformSite(ctx, robotDeepCopy, task.MapName, finalTarget)
		if err != nil || finalNode == nil {
			log.Errorf("任务模板生成子任务错误，申请称重平台点失败  taskId:%d", task.Id)
			return nil, err, passIndexArr
		}
		if finalNode == nil {
			log.Errorf("任务模板生成子任务错误，找不到称重平台点  taskId:%d", task.Id)
			return nil, err, passIndexArr
		}
		finalTarget = finalNode.Id
	}

	//找出点
	siteId := task.TargetSiteID
	//3、根据模板生成子任务
	lastItem := -1
	for _, taskItemTemplate1 := range taskItemTemplates {
		taskItemTemplate := *taskItemTemplate1
		taskItemTemplate.PalletDetection = model.PalletDetectionTypeNone
		taskItemTemplate.TargetDirection = 0
		//判断货叉方向
		if taskItemTemplate.TaskType == model.TaskTypePick || taskItemTemplate.TaskType == model.TaskTypePut {
			taskItemTemplate.TargetDirection = task.TargetDirection
		}

		//点位类型不为上一条任务的终点时，重置任务点
		if taskItemTemplate.TargetType != model.TargetTypeNow {
			siteId = finalTarget
		}
		//判断是否需要前往旋转点(货叉方向、小车类型)
		switch taskItemTemplate.TargetType {
		//任务终点02
		case model.TargetTypeTaskEnd:
			siteId = finalTarget
		//任务终点前一个点
		case model.TargetTypeTaskEndFront:
			if taskItemTemplate.TaskType != model.TaskTypeDetection {
				if robot.Type.CategoryType == model.CategoryTypeStocker || robot.Type.CategoryType == model.CategoryTypeFronted {
					// if robot.Type.CategoryType == model.CategoryTypeStocker {
					if robot.Status.SiteIdS != finalTarget {
						// log.Infof("非原地取放货任务 task-siteId = %d robot-siteId = %d", task.TargetSiteID, robot.Status.SiteId)
						continue
					}
				}
			}
			node, err := m.TRM().GetTargetFront(task.MapName, finalTarget, robot)
			if err != nil {
				log.Errorf("任务模板生成子任务错误，获取任务前一个点失败  taskId:%d", task.Id)
				return nil, err, passIndexArr
			}
			siteId = node.Id
			task.TargetFrontSiteId = siteId

		//旋转点
		case model.TargetTypeRotate:
			if robot.Type.CategoryType != model.CategoryTypeThreeWay {
				log.Errorf("任务模板生成子任务错误，子任务模板配置错误(非三项叉不可执行旋转货叉模板)  taskId:%d", task.Id)
				return nil, codes.ErrTemplateNotMatch, passIndexArr
			}

			//log.Infof("子任务模板生成子任务时，小车货叉方向:%d  任务货叉方向:%d", robot.Status.ForkDirection, task.TargetDirection)
			if task.TargetDirection == robot.Status.ForkDirection {
				continue
			}
			taskItemTemplate.TargetDirection = task.TargetDirection
			//判断终点前一个点
			front_node, err := m.TRM().GetTargetFront(task.MapName, finalTarget, robot)
			if err != nil {
				log.Errorf("任务模板生成子任务错误，获取任务前一个点失败  taskId:%d", task.Id)
				return nil, err, passIndexArr
			}
			//获取可到达的旋转点信息
			//深拷贝
			if !robot.StatusIsExists() {
				log.Errorf("robot:%s 状态不存在（taskId:%d）", robot.Account.AgvId, task.Id)
				return nil, codes.ErrRobotStatusNotExists, passIndexArr
			}
			var robotDeepCopy model.Robot
			temp, err123 := json.Marshal(robot)
			if err123 != nil {
				return nil, codes.ErrRobotStatusNotExists.WithDetails(err123.Error()), passIndexArr
			}
			err123 = json.Unmarshal(temp, &robotDeepCopy)
			if err123 != nil {
				return nil, codes.ErrRobotStatusNotExists.WithDetails(err123.Error()), passIndexArr
			}

			node, err := m.TRM().MallocRotatePoint(ctx, &robotDeepCopy, task.MapName, front_node.Id, m.normalTaskLock[robot.Account.MapName])
			if err != nil || node == nil {
				log.Errorf("任务模板生成子任务错误，申请旋转点失败  taskId:%d", task.Id)
				return nil, err, passIndexArr
			}
			siteId = node.Id

		//小车当前点
		case model.TargetTypeRobot:
			siteId = robot.Status.SiteIdS

		case model.TargetTypeTaskEndFrontPoint:
			node, err := m.TRM().GetTargetFront(task.MapName, finalTarget, robot)
			if err != nil {
				log.Errorf("任务模板生成子任务错误，获取任务前一个点失败  taskId:%d", task.Id)
				return nil, err, passIndexArr
			}
			siteId = node.Id
			task.TargetFrontSiteId = siteId
		}

		//三项叉、侧叉车取放货
		if (task.Type == model.TaskTypePut || task.Type == model.TaskTypePick) &&
			(robot.Type.CategoryType == model.CategoryTypeThreeWay || robot.Type.CategoryType == model.CategoryTypeSideFork) {
			taskItemTemplate.TargetDirection = task.TargetDirection
		}
		if task.PalletDetection != model.PalletDetectionTypeNone && taskItemTemplate.TaskType == model.TaskTypeDetection {
			taskItemTemplate.PalletDetection = task.PalletDetection
		}
		subtask.ConvertByTemplate(*task, taskItemTemplate, robot, task.MapName, siteId)
		subtasks = append(subtasks, *subtask)
		index := len(subtasks) - 1
		//-----------------------
		if lastItem == int(model.TargetTypeTaskEndFrontPoint) || lastItem == int(model.TargetTypeTaskEndFront) {
			if taskItemTemplate.TargetType == model.TargetTypeTaskEndFrontPoint || taskItemTemplate.TargetType == model.TargetTypeTaskEndFront {
				continue
			}
		}
		if lastItem-int(taskItemTemplate.TargetType) != 0 {
			lastItem = int(taskItemTemplate.TargetType)
			passIndexArr = append(passIndexArr, index)
		}

	}

	return subtasks, nil, passIndexArr
}

// getRobotsMapsMap 获取机器人能接单的地图map
func (m *DispatchManager) getRobotsMapsMap(ctx context.Context, robots []*model.Robot) (map[string][]*model.Robot, codes.Code) {
	groups := robotmanager.GetAllRobotGroup()
	groupMap := make(map[string]*model.RobotGroup)
	for _, g := range groups {
		// log.Infof("%v", g.Id.String()[10:len(g.Id.String())-2])
		groupMap[g.Id.Hex()] = g
	}

	maps := m.mc.TRM.GetAllMapNames()

	if maps == nil || len(maps) < 1 {
		return nil, nil
	}

	mapData := make(map[string][]*model.Robot)
	for _, n := range maps {
		mapData[n] = make([]*model.Robot, 0)
	}

	for _, r := range robots {
		var robotMaps []string
		robot := r
		if robot.Account.GroupID != "" {
			groupInfo, ok := groupMap[robot.Account.GroupID]
			if ok {
				robotMaps = groupInfo.MapList
			}
		}

		if robotMaps == nil || len(robotMaps) == 0 {
			for _, mapName := range maps {
				if robot.Account.MapName == mapName {
					mapData[mapName] = append(mapData[mapName], robot)
				} else {
					if m.TRM().CrossMapCanReach(mapName, robot.Account.MapName) {
						mapData[mapName] = append(mapData[mapName], robot)
					}
				}
			}
			continue
		}

		for _, robotMapName := range robotMaps {
			mapData[robotMapName] = append(mapData[robotMapName], robot)
		}
	}
	return mapData, nil
}

// getCanUseRobots 获取当前可用的机器人
func (m *DispatchManager) getCanUseRobots(ctx context.Context) []*model.Robot {
	canUseRobots := make([]*model.Robot, 0)
	robots := robotmanager.GetAllTaskRobotsFromCache()
	for _, robot := range robots {
		if !robot.StatusIsExists() {
			log.Errorf("robot %s status is nil", robot.Account.AgvId)
			continue
		}

		if !robot.Status.Online || !robot.Status.TaskOnline {
			log.Warnf("robot %s status %s:%s is not online", robot.Account.AgvId, robot.Status.Online, robot.Status.TaskOnline)
			continue
		}

		//数据库查询主任务与小车绑定信息
		mainTask, err := m.mc.TM.GetMainTaskByAgvFromDB(ctx, robot.Account.AgvId)
		if err != nil {
			log.Errorf("数据库查询主任务与小车%s绑定信息错误 error:%s", err.UnWrapError(), robot.Account.AgvId)
			continue
		}

		robot.Allocated = false //初始化，机器人当前都没有背分配过
		//小车没绑定任务可用
		if mainTask == nil {
			canUseRobots = append(canUseRobots, robot)
			continue
		}

		//小车已经绑定任务，等待解析状态不可用
		if mainTask.TaskId == 0 || mainTask.MainTaskCode != "" {
			continue
		}

		nowTask, err := m.mc.TM.GetTaskFromDB(ctx, mainTask.TaskId)
		if err != nil || nowTask == nil {
			log.Errorf("获取主任务的任务%s错误error:%s", mainTask.TaskId, err.UnWrapError())
			continue
		}

		// 充电,待命的机器人是可用的
		if nowTask.Type == model.TaskTypeCharge || nowTask.Type == model.TaskTypeStandBy || nowTask.Type == model.TaskTypeAvoid {
			canUseRobots = append(canUseRobots, robot)
		}
	}
	return canUseRobots
}
func (m *DispatchManager) RobotGpathsLess(ctx context.Context, sn string) bool {
	dbRes := m.db.Collection(ctx, model.GlobalPath{}.TableName()).FindOne(ctx, bson.M{"sn": sn})
	if dbRes.Err() != nil {
		return false
	}
	res := new(model.GlobalPath)
	if errBind := dbRes.Decode(res); errBind != nil {
		return false
	}
	return len(res.Paths) < 3 && len(res.Paths) > 0
}
func (m *DispatchManager) findBestRobot(ctx context.Context, task *model.Task, robots []*model.Robot, useMultiPlaner bool) (*model.Robot, []model.Subtask) {
	log.Warnf("findBestRobot,rms_task_id:%d", task.Id)
	startTime := time.Now()
	defer func() {
		log.Warnf("taskId:%d handle findBestRobot time %dms", task.Id, time.Now().UnixMilli()-startTime.UnixMilli())
	}()
	gotAdditiveInfo := false
	gotGategoryID := 0
	if task.AdditiveInfo != "" && task.AdditiveInfo != "string" {
		// log.Errorf("任务附加信息,类型%s", task.AdditiveInfo)
		categoryID, err := strconv.Atoi(task.AdditiveInfo)
		if err != nil {
			log.Errorf("任务附加信息无法转为Int，数据为%s", task.AdditiveInfo)
			return nil, nil
		}
		_, errGetType := robotmanager.GetTypeFromDB(ctx, categoryID)
		if errGetType != nil {
			log.Errorf("任务附加信息%d指定的类别不存在", categoryID)
			return nil, nil
		}
		gotAdditiveInfo = true
		gotGategoryID = categoryID
	}
	results := solvedRobots{}
	heap.Init(&results)
	for _, robot := range robots {
		//当前任务组号不包含该车
		if task.GroupList != nil && len(task.GroupList) > 0 && robot.Account.GroupID != "" {
			//判断是否包含
			if !tools.StringSliceIsContain(task.GroupList, robot.Account.GroupID) {
				continue
			}
			if task.AgvId == "" {
				//类型相同，判断相关区域类是否存在指定的车，或者目标点也在该区域的车
				errR, getRobot := m.mc.GetRobotInLimitetAreaByGroupList(task.GroupList, *task)
				if errR != nil {
					log.Infof("限制分车出现错误:%v,AGV:%s,type:%d", errR, robot.Account.AgvId, robot.Type.CategoryId)
					continue
				}
				if getRobot != nil {
					//找到车，判断当前计算的车是否和得到的车一致
					log.Warnf("任务限定区域（按照分组）找出执行可执行AGV,AGV:%s", robot.Account.AgvId)
					if getRobot.Account.AgvId != robot.Account.AgvId {
						continue
					}
				}
			}
		}
		if gotAdditiveInfo {
			if robot.Account.CategoryID != gotGategoryID {
				log.Infof("正确类型%d,AGV:%s,type:%d", gotGategoryID, robot.Account.AgvId, robot.Type.CategoryId)
				continue
			}
			//
			if robot.Type.CategoryType == model.CategoryTypeThreeWay {
				if task.AgvId == "" {
					//类型相同，判断相关区域类是否存在指定的车，或者目标点也在该区域的车
					log.Warnf("任务：%d,robotID:%d 类型：%d,开始在限定区域找车,", task.Id, robot.Account.AgvId, gotGategoryID)
					errR, getRobot := m.mc.GetRobotInLimitetAreaByRobotType(gotGategoryID, *task)
					if errR != nil {
						log.Infof("限制分车出现错误:%v,AGV:%s,type:%d", errR, robot.Account.AgvId, robot.Type.CategoryId)
						continue
					}
					log.Warnf("任务：%d,robotID:%d 类型：%d,开始在限定区域找车，找车结果:%v,", task.Id, robot.Account.AgvId, gotGategoryID, getRobot)
					if getRobot != nil {
						//找到车，判断当前计算的车是否和得到的车一致
						log.Warnf("任务限定区域（指定的AGV类型）找出执行可执行AGV,AGV:%s", robot.Account.AgvId)
						if getRobot.Account.AgvId != robot.Account.AgvId {
							continue
						}
					}
				}
			}
		}
		if task.AgvId != "" && robot.Account.AgvId != task.AgvId {
			//log.Infof("task:%d指定的机器人%s不匹配", task.Id, task.AgvId)
			continue
		}
		if !robot.StatusIsExists() {
			continue
		}
		// 电量不够不可用,已经分配车的不管电量
		if task.Type != model.TaskTypeCharge && robot.Status.Power < robot.Type.ChargePercent {
			log.Infof("task:%d非充电任务,机器人%s电量%d过低%d", task.Id, robot.Account.AgvId, robot.Status.Power, robot.Type.ChargePercent)
			continue
		}
		mainTask, _ := m.mc.TM.GetMainTaskByAgvFromDB(ctx, robot.Account.AgvId)
		if mainTask != nil && mainTask.MainTaskCode != "" && task.MainTaskCode != mainTask.MainTaskCode {
			continue
		}

		var subtaskFunc GetMapSubtasks
		if !useMultiPlaner {
			if robot.Account.MapName == task.MapName {
				subtaskFunc = m.GetSameMapSubtasks
			} else {
				continue
			}
		} else {
			subtaskFunc = m.GetDifferentMapSubtasks
		}
		subtasks, cost, err := subtaskFunc(ctx, robot, task)

		if err != nil {
			// log.Errorf("模板生成子任务错误 error:%s", err.Error())
			continue
		}
		log.Warnf("RMS任务:%d 机器人编号:%s 代价数值:%f", task.Id, robot.Account.AgvId, cost)
		heap.Push(&results, &taskSolveRes{robot: robot, cost: cost, robotSubtasks: subtasks})
	}

	//尝试中断任务
	//获取所有可用AGV
	if len(results) <= 0 {
		// log.Infof("task:%d没有可用的机器人", task.Id)
		return nil, nil
	}
	for _, res := range results {
		runningSubtask, _ := m.mc.TM.GetRunningSubtaskFromDB(ctx, res.robot.Account.AgvId)
		if runningSubtask == nil {
			runningSubtask, _ = m.mc.TM.GetWaitingSubtaskFromDB(ctx, res.robot.Account.AgvId)
			if runningSubtask == nil {
				avoidTask, _ := m.mc.TM.GetRobotIdleAvoidTaskFromDB(context.Background(), res.robot.Account.AgvId)
				if avoidTask != nil && avoidTask.Id != task.Id {
					continue
				}
				res.robot.AllocateLock()
				defer res.robot.AllocateUnLock()
				if res.robot.Allocated {
					log.Infof("机器人%s已经被锁定", res.robot.Account.AgvId)
					continue
				}
				res.robot.Allocated = true
				return res.robot, res.robotSubtasks
			}
		}
		if runningSubtask.Type == model.TaskTypeStandBy || runningSubtask.Type == model.TaskTypeAvoid || runningSubtask.Type == model.TaskTypeCharge {
			if time.Since(runningSubtask.StartTime) < 3*time.Second || time.Since(runningSubtask.CreatedTime) < 5*time.Second {
				log.Infof("给小车(%s)下发任务(%d)时间短，不允许中断", res.robot.Account.AgvId, runningSubtask.TaskId)
				continue
			}
		}
		switch {
		//待命任务中断
		case runningSubtask.Type == model.TaskTypeStandBy:
			log.Infof("任务:%d,尝试中断待命任务,机器人:%s状态:%d", runningSubtask.Id, res.robot.Account.AgvId, res.robot.Status.Status)
			if errBreak := m.breakTask(runningSubtask, nil); errBreak != nil {
				log.Errorf("break robot:%s task:%d error:%s", res.robot.Account.AgvId, runningSubtask.Id, errBreak.UnWrapError())
				continue
			}
			return nil, nil
		case runningSubtask.Type == model.TaskTypeAvoid:
			if res.robot.StatusIsExists() && (m.RobotGpathsLess(ctx, res.robot.Account.AgvId) || res.robot.Status.SiteIdS == runningSubtask.TargetSiteID) {
				log.Infof("给小车(%s)下发避让任务(%d)剩余路径短，不进行中断", res.robot.Account.AgvId, runningSubtask.TaskId)
				continue
			}
			log.Infof("任务:%d,尝试中断避让任务,机器人:%s状态:%d", runningSubtask.Id, res.robot.Account.AgvId, res.robot.Status.Status)
			if errBreak := m.breakTask(runningSubtask, nil); errBreak != nil {
				log.Errorf("break robot:%s task:%d error:%s", res.robot.Account.AgvId, runningSubtask.Id, errBreak.UnWrapError())
				continue
			}
			return nil, nil
		//充电任务中断
		case runningSubtask.Type == model.TaskTypeCharge:
			//先根据子任务获得rms任务，判断
			chargerTask, err1 := m.mc.TM.GetTaskFromDB(ctx, runningSubtask.TaskId)
			if err1 != nil {
				log.Errorf("获得即将打断的充电子任务:%d rms任务Id:%d 错误:%s", runningSubtask.Id, runningSubtask.TaskId, err1.Error())
				continue
			}
			if chargerTask == nil {
				log.Errorf("获得即将打断的充电，子任务:%d rms任务Id:%d 错误:根据子任务查询RMS任务为空", runningSubtask.Id, runningSubtask.TaskId)
				continue
			}
			if chargerTask.ChargerMode == model.CharerModeTypeTimerRule {
				//判断是否超过可打断时间
				currentBeijinTime := tools.GetCurrentTiemByshanghai()
				ruleEndTime := tools.GetCurrentTiemByshanghai1(chargerTask.ChargerRule.EndTime)
				if currentBeijinTime.Sub(ruleEndTime) <= 0 {
					continue
				}

			}

			//判断充电桩状态
			if res.robot.Status.Power > res.robot.Type.ChargeToTaskPercent {
				log.Infof("任务:%d,尝试中断正常充电任务,机器人:%s机器人状态:%d", runningSubtask.Id, res.robot.Account.AgvId, res.robot.Status.Status)
				if errBreak := m.breakTask(runningSubtask, nil); errBreak != nil {
					log.Errorf("break robot:%s task:%d error:%s", res.robot.Account.AgvId, runningSubtask.Id, errBreak.UnWrapError())
					continue
				}
				return nil, nil
			} else {
				continue
			}
		}

		res.robot.AllocateLock()
		defer res.robot.AllocateUnLock()
		if res.robot.Allocated {
			log.Infof("机器人%s已经被锁定", res.robot.Account.AgvId)
			continue
		}
		res.robot.Allocated = true
		return res.robot, res.robotSubtasks
	}
	return nil, nil
}

//获取同地图路径cost和subtasks
func (m *DispatchManager) GetSameMapSubtasks(ctx context.Context, robot *model.Robot, task *model.Task) ([]model.Subtask, float64, codes.Code) {
	// globalRejects, globalRejectsAll, localRejects := m.mc.MakePlanRejectNodes(robot.Account.AgvId, task.MapName, robot.AgvCorverNodes, nil)
	subtasks, err, passIndexArr := m.GetSubTasks(ctx, task, robot)
	if err != nil {
		log.Errorf("模板生成子任务错误 error:%s", err.UnWrapError())
		return nil, 0, err
	}
	if subtasks == nil || len(subtasks) < 1 {
		//log.Errorf("模板生成子任务错误")
		return nil, 0, codes.ErrSubtasksIsNull
	}
	globalRejects, globalRejectsAll, localRejects := m.mc.MakePlanRejectNodes(robot.Account.AgvId, task.MapName, robot.AgvCorverNodes, nil)
	if subtasks[0].TargetSiteID != task.TargetSiteID {
		//判断目标点是否可达
		palner := m.mc.TRM.GetPathPlanner(task.MapName)
		if palner == nil {
			log.Errorf("robot:%s,RMS任务编号：%d ,找不到地图:%s规划器", robot.Account.AgvId, task.Id, task.MapName)
			return nil, 0, codes.ErrPlanerNotExit
		}
		targetRejects := m.mc.TargetPlanRejectNodes(robot.Account.AgvId, task.MapName)
		if _, find := targetRejects[task.TargetSiteID]; find {
			log.Errorf("robot:%s,RMS任务编号：%d ,因为最终目标点被剔除（禁行区域,或者被手动锁定），所以路径计算不同", robot.Account.AgvId, task.Id, task.MapName)
			return nil, 0, codes.ErrPathPlanError
		}

	}
	gPaths, _, _, _, _, errGPlan, _, gPurePaths := m.mc.TRM.PathPlan(ctx, robot, &subtasks[0], globalRejects, globalRejectsAll, localRejects, false, m.SystemConfig.TrafficCfg.UseTaskAvoid, false, false)
	cost := 0.0
	if errGPlan != nil || (gPurePaths == nil && robot.Status.SiteIdS != subtasks[0].TargetSiteID) {
		return nil, 0, errGPlan
	}
	if gPurePaths != nil {
		cost = gPurePaths.Cost
	}
	cost1 := 0.0
	if robot.Type.CategoryType == model.CategoryTypeThreeWay && robot.StatusIsExists() && len(passIndexArr) > 1 {
		copyFirstRobot := robot.GetDeepCopyRobot()
		var nextRobot *model.Robot
		nextRobot = copyFirstRobot
		//对于三相叉计算后面的带价值
		for _, passIndex := range passIndexArr {
			cost2 := 0.0
			//目标点
			nextTargetSiteId := subtasks[passIndex].TargetSiteID
			if nextRobot.Status.SiteIdS != subtasks[passIndex].TargetSiteID {
				//计算下一段带价值
				gpn, errn, targetCarPosen := m.mc.TRM.GetNextSubTaskCost(subtasks[passIndex].MapName, nextTargetSiteId, nextRobot)
				if errn != nil {
					return nil, 0, errn
				}
				if gpn == nil {
					return nil, 0, codes.ErrPathPlanError
				}
				//下个姿态的Robot
				nextRobot = copyFirstRobot.GetNextTargetSiteIdDeepCopyRobot(nextTargetSiteId, *targetCarPosen)
				//
				if passIndex == 0 {
					continue
				}
				cost2 = gpn.Cost

			}

			cost1 = cost2 + cost1
		}
		log.Warnf("sn:%s is threeWay , next paths cost:%f", robot.Account.AgvId, cost1)
	}
	log.Infof("sn:%s path:%v cost:%f  (additional cost:%f)", robot.Account.AgvId, gPaths.Paths.ToIDs(), cost, cost1)
	return subtasks, (cost + cost1), nil
}

//获取不同地图路径cost和subtasks
func (m *DispatchManager) GetDifferentMapSubtasks(ctx context.Context, robot *model.Robot, task *model.Task) ([]model.Subtask, float64, codes.Code) {
	elevators := m.DM().GetAllElevatorFromCache()
	allRobots := robotmanager.GetAllRobotsFromCache()
	goalList, cost, err := m.TRM().GetCrossPath(ctx, robot, task.MapName, task.TargetSiteID, elevators, allRobots)
	if err != nil {
		// log.Errorf("task:%d robot:%s 模板生成子任务错误 error:%s", task.Id, robot.Account.AgvId, err.UnWrapError())
		return nil, 0, err
	}
	//模板生成子任务
	subtasks, err := m.GetCrossMapSubTasks(ctx, goalList, task, robot)
	if err != nil {
		// log.Errorf("模板生成子任务错误 error:%s", err.UnWrapError())
		return nil, 0, err
	}
	return subtasks, cost, nil
}

func (m *DispatchManager) GetCrossMapSubTasks(ctx context.Context, goalList []model.TaskInfo, task *model.Task, robot *model.Robot) ([]model.Subtask, codes.Code) {
	subtasks := make([]model.Subtask, 0)
	for i, g := range goalList {
		subtask := model.Subtask{
			TaskId:               task.Id,
			Index:                i,
			Type:                 model.TaskTypeNavi,
			Status:               model.TaskStatusAssign,
			MapName:              g.TaskMapName,
			TargetSiteID:         g.TaskSiteId,
			TargetLayer:          0,
			TargetLayerCopy:      0,
			TargetDirection:      0,
			Grade:                task.Grade,
			AgvId:                robot.Account.AgvId,
			ForkActions:          []model.ForkAction{},
			CompleteProcessType:  0,
			CompleteProcessValue: "",
			IsCompleteProcess:    0,
			AdditiveInfo:         "",
			CreatedTime:          time.Now(),
			WalkType:             0,
			WalkTypeCopy:         0,
			NextSwitchType:       g.SwitchType,
			NextSwitchDevice:     g.DeviceCode,
			NextSwitchDeviceCopy: g.DeviceCode,
			// SwitchToMap:          g.ToMapName,
			// SwitchToSite:         g.ToMapSite,
			// SwitchToPath:         g.ToMapPath,
		}
		if g.SwitchType == model.FactoryRealtion {
			log.Warnf("子任务序号:%d 任务地图:%s 目标点:%s 切换地图:%s 切换设备:%s 切换点位:%s 切换路径:%s 切换类别:跨厂区  robot%s", i, subtask.MapName, subtask.TargetSiteID,
				g.ToMapName, g.DeviceCode, g.ToMapSite, g.ToMapPath, robot.Account.AgvId)
		} else {
			log.Warnf("子任务序号:%d 任务地图:%s 目标点:%s 切换地图:%s 切换设备:%s 切换点位:%s 切换路径:%s 切换类别:跨楼层  robot%s", i, subtask.MapName, subtask.TargetSiteID,
				g.ToMapName, g.DeviceCode, g.ToMapSite, g.ToMapPath, robot.Account.AgvId)
		}

		subtasks = append(subtasks, subtask)
	}
	final_subtasks, err, _ := m.GetSubTasks(ctx, task, robot)
	if err != nil || subtasks == nil || len(subtasks) < 1 {
		log.Errorf("模板生成子任务错误 error:%s", err)
		return nil, err
	}
	subtasks = append(subtasks, final_subtasks...)
	for i, _ := range subtasks {
		if i > 0 && i <= len(goalList) {
			subtasks[i].SwitchType = goalList[i-1].SwitchType
			subtasks[i].SwitchDevice = goalList[i-1].DeviceCode
			subtasks[i].SwitchDeviceCopy = goalList[i-1].DeviceCode
			subtasks[i].SwitchToSite = goalList[i-1].ToMapSite
			subtasks[i].SwitchFromMap = goalList[i-1].TaskMapName
			subtasks[i].SwitchFromSite = goalList[i-1].TaskSiteId
			subtasks[i].SwitchToPath = goalList[i-1].ToMapPath
		}
	}
	return subtasks, nil
}
